So recently we updated our Driver Station to the newest version found here
Ever since we updated our Driver Station only reads “System: WatchDog”
instead of “System: Enabled”
I eventually stumbled across this page Archived: [FRC 2014] An Overview of FRC Watchdogs - NI Community
So I ran through the checklist
- The competition dongle is attached
- I pinged every single ip and everything is connected properly
- All of the Software has been updated to the latest and greatest
**Our cRIO is updated to the latest version found here
Our Windriver Workbench is running on the newest version 3.0.1718**
The last thing on the checklist is
If the system watchdog is working, but your specific code is not, the user watchdog is the culprit. This can be diagnosed with the debugging tools available in your development environment such as breakpoints. Verify that the user watchdog is being fed within its previously defined time limit.
and I am not sure how to do it. Any suggestions are appreciated!
Ignore the following unless you think the code is the problem
#include "WPILib.h"
#include "vxWorks.h"
#include "Encoder.h"
#include "DigitalInput.h"
#include "Resource.h"
#include "Utility.h"
#include "WPIStatus.h"
#include "Relay.h"
#include "DigitalModule.h"
#include "stdio.h"
#include <math.h>
#include "Accelerometer.h"
#include "timers.h"
#include <iostream.h>
#include "AxisCamera.h"
#include "BaeUtilities.h"
#include "FrcError.h"
#include "TrackAPI.h"
#include "Target.h"
// To locally enable debug printing: set the debugFlag to a 1, to disable set to 0
static int TwoColorDemo_debugFlag = 0;
#define DPRINTF if(TwoColorDemo_debugFlag)dprintf
#define PI 3.14159265358979
// for 160x120, 50 pixels = .38 %
#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25 // target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 20.0 // target is too close
/** Simple test to see if the color is taking up too much of the image */
int tooClose(ParticleAnalysisReport* par) {
if (par->particleToImagePercent > MAX_PARTICLE_TO_IMAGE_PERCENT)
return 1;
return 0;
}
/** Simple test to see if the color is large enough */
int bigEnough(ParticleAnalysisReport* par) {
if (par->particleToImagePercent < MIN_PARTICLE_TO_IMAGE_PERCENT)
return 0;
return 1;
}
class BuiltinDefaultCode : public IterativeRobot
{
// Declare variable for the robot drive system
RobotDrive *m_robotDrive; // robot will use PWM 1-2 for drive motors
RobotDrive *m_Shooter; // Robot Shoots with PWM 3-4
RobotDrive *m_Lift; // Robot will use PWM 5 & 6 for the Lift Motors
// Declare Servo's & Tracking Variables
Servo *horizontalServo; // first servo object
Servo *verticalServo; // second servo object
float horizontalDestination; // servo destination (0.0-1.0)
float verticalDestination; // servo destination (0.0-1.0)
float horizontalPosition, verticalPosition; // current servo positions
float servoDeadband; // percentage servo delta to trigger move
int framesPerSecond; // number of camera frames to get per second
float panControl; // to slow down pan
double sinStart; // control where to start the sine wave input for pan
TrackingThreshold td1, td2; // color thresholds
ParticleAnalysisReport par1, par2; // particle analysis reports
// Declare Encoder Variables for Input
Encoder *encoderMotor1;
Encoder *encoderMotor2;
// Declare Accelerometer
Accelerometer *accl;
// Declare FlipperDoor Variable
Relay *flipperdoor; // Flipperdoor
// Declare GearTooth
DigitalInput *flipperdoorright;
DigitalInput *flipperdoorleft;
// Declare Gyro
Gyro* gyro;
// Declare a variable to use to access the driver station object
DriverStation *m_ds; // driver station object
UINT32 m_priorPacketNumber; // keep track of the most recent packet number from the DS
UINT8 m_dsPacketsReceivedInCurrentSecond; // keep track of the ds packets received in the current second
// Declare variables for the two joysticks being used
Joystick *m_rightStick; // joystick 1 (arcade stick or right tank stick)
Joystick *m_leftStick; // joystick 2 (tank left stick)
static const int NUM_JOYSTICK_BUTTONS = 16;
bool m_rightStickButtonState(NUM_JOYSTICK_BUTTONS+1)];
bool m_leftStickButtonState(NUM_JOYSTICK_BUTTONS+1)];
// Declare variables for each of the eight solenoid outputs
static const int NUM_SOLENOIDS = 8;
Solenoid *m_solenoids(NUM_SOLENOIDS+1)];
enum { // drive mode selection
UNINITIALIZED_DRIVE = 0,
ARCADE_DRIVE = 1,
TANK_DRIVE = 2
} m_driveMode;
// Local variables to count the number of periodic loops performed
UINT32 m_autoPeriodicLoops;
UINT32 m_disabledPeriodicLoops;
UINT32 m_telePeriodicLoops;
public:
/**
* Constructor for this "BuiltinDefaultCode" Class.
*
* The constructor creates all of the objects used for the different inputs and outputs of
* the robot. Essentially, the constructor defines the input/output mapping for the robot,
* providing named objects for each of the robot interfaces.
*/
BuiltinDefaultCode(void) {
printf("BuiltinDefaultCode Constructor Started
");
// Create a robot using standard right/left robot drive on PWMS 1, 2, 3, and #4
m_robotDrive = new RobotDrive(1,2);
m_Shooter = new RobotDrive(3,4);
m_Lift = new RobotDrive(5,6);
// remember to use jumpers on the sidecar for the Servo PWMs
horizontalServo = new Servo(9); // create horizontal servo on PWM 9
verticalServo = new Servo(10); // create vertical servo on PWM 10
servoDeadband = 0.01; // move if > this amount
framesPerSecond = 15; // number of camera frames to get per second
sinStart = 0.0; // control where to start the sine wave for pan
memset(&par1,0,sizeof(ParticleAnalysisReport)); // initialize particle analysis report
memset(&par2,0,sizeof(ParticleAnalysisReport)); // initialize particle analysis report
/* image data for tracking - override default parameters if needed */
/* recommend making PINK the first color because GREEN is more
* subsceptible to hue variations due to lighting type so may
* result in false positives */
// PINK
sprintf (td1.name, "PINK");
td1.hue.minValue = 220;
td1.hue.maxValue = 255;
td1.saturation.minValue = 75;
td1.saturation.maxValue = 255;
td1.luminance.minValue = 85;
td1.luminance.maxValue = 255;
// GREEN
sprintf (td2.name, "GREEN");
td2.hue.minValue = 55;
td2.hue.maxValue = 125;
td2.saturation.minValue = 58;
td2.saturation.maxValue = 255;
td2.luminance.minValue = 92;
td2.luminance.maxValue = 255;
/* set up debug output:
* DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE
*/
SetDebugFlag(DEBUG_SCREEN_ONLY);
/* start the CameraTask */
if (StartCameraTask(framesPerSecond, 0, k320x240, ROT_0) == -1) {
DPRINTF( LOG_ERROR,"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()) );
}
/* allow writing to vxWorks target */
Priv_SetWriteFileAllowed(1);
/* stop the watchdog if debugging */
GetWatchdog().SetExpiration(0.5);
GetWatchdog().SetEnabled(false);
// Encoders
encoderMotor1 = new Encoder(3,4);
encoderMotor2 = new Encoder(5,6);
// Accelerometer
accl = new Accelerometer(7,8);
// Flipper Door
flipperdoor = new Relay(1);
flipperdoorleft = new DigitalInput(1);
flipperdoorright = new DigitalInput(2);
// Acquire the Driver Station object
m_ds = DriverStation::GetInstance();
m_priorPacketNumber = 0;
m_dsPacketsReceivedInCurrentSecond = 0;
// Define joysticks being used at USB port #1 and USB port #2 on the Drivers Station
m_rightStick = new Joystick(1);
m_leftStick = new Joystick(2);
// Iterate over all the buttons on each joystick, setting state to false for each
UINT8 buttonNum = 1; // start counting buttons at button 1
for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
m_rightStickButtonState[buttonNum] = false;
m_leftStickButtonState[buttonNum] = false;
}
// Set drive mode to uninitialized
m_driveMode = UNINITIALIZED_DRIVE;
// Initialize counters to record the number of loops completed in autonomous and teleop modes
m_autoPeriodicLoops = 0;
m_disabledPeriodicLoops = 0;
m_telePeriodicLoops = 0;
printf("BuiltinDefaultCode Constructor Completed
");
}
/********************************** Init Routines *************************************/
void RobotInit(void) {
printf("RobotInit() completed.
");
EncoderInit();
}
void DisabledInit(void) {
m_disabledPeriodicLoops = 0; // Reset the loop counter for disabled mode
// Move the cursor down a few, since we'll move it back up in periodic.
printf("\x1b2B");
}
void AutonomousInit(void) {
m_autoPeriodicLoops = 0; // Reset the loop counter for autonomous mode
}
void TeleopInit(void) {
m_telePeriodicLoops = 0; // Reset the loop counter for teleop mode
m_dsPacketsReceivedInCurrentSecond = 0; // Reset the number of dsPackets in current second
m_driveMode = UNINITIALIZED_DRIVE; // Set drive mode to uninitialized
}
void EncoderInit(void) {
encoderMotor1->Start();
encoderMotor2->Start();
}
/********************************** Servo Routines ****************************************/
/**
* Set servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0).
*
* @param normalizedHorizontal Pan Position from -1.0 to 1.0.
* @param normalizedVertical Tilt Position from -1.0 to 1.0.
*/
void setServoPositions(float normalizedHorizontal, float normalizedVertical) {
float servoH = NormalizeToRange(normalizedHorizontal);
float servoV = NormalizeToRange(normalizedVertical);
float currentH = horizontalServo->Get();
float currentV = verticalServo->Get();
/* make sure the movement isn't too small */
if ( fabs(servoH - currentH) > servoDeadband ) {
horizontalServo->Set( servoH );
/* save new normalized horizontal position */
horizontalPosition = RangeToNormalized(servoH, 1);
}
if ( fabs(servoV - currentV) > servoDeadband ) {
verticalServo->Set( servoV );
verticalPosition = RangeToNormalized(servoV, 1);
}
}
/**
* Adjust servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0).
*
* @param normalizedHorizontal Pan adjustment from -1.0 to 1.0.
* @param normalizedVertical Tilt adjustment from -1.0 to 1.0.
*/
void adjustServoPositions(float normDeltaHorizontal, float normDeltaVertical) {
/* adjust for the fact that servo overshoots based on image input */
normDeltaHorizontal /= 8.0;
normDeltaVertical /= 4.0;
/* compute horizontal goal */
float currentH = horizontalServo->Get();
float normCurrentH = RangeToNormalized(currentH, 1);
float normDestH = normCurrentH + normDeltaHorizontal;
/* narrow range keep servo from going too far */
if (normDestH > 1.0) normDestH = 1.0;
if (normDestH < -1.0) normDestH = -1.0;
/* convert input to servo range */
float servoH = NormalizeToRange(normDestH);
/* compute vertical goal */
float currentV = verticalServo->Get();
float normCurrentV = RangeToNormalized(currentV, 1);
float normDestV = normCurrentV + normDeltaVertical;
if (normDestV > 1.0) normDestV = 1.0;
if (normDestV < -1.0) normDestV = -1.0;
/* convert input to servo range */
float servoV = NormalizeToRange(normDestV, 0.2, 0.8);
/* make sure the movement isn't too small */
if ( fabs(currentH-servoH) > servoDeadband ) {
horizontalServo->Set( servoH );
/* save new normalized horizontal position */
horizontalPosition = RangeToNormalized(servoH, 1);
}
if ( fabs(currentV-servoV) > servoDeadband ) {
verticalServo->Set( servoV );
verticalPosition = RangeToNormalized(servoV, 1);
}
}
/********************************** Periodic Routines *************************************/
void DisabledPeriodic(void) {
static INT32 printSec = (INT32)GetClock() + 1;
static const INT32 startSec = (INT32)GetClock();
// feed the user watchdog at every period when disabled
GetWatchdog().Feed();
// increment the number of disabled periodic loops completed
m_disabledPeriodicLoops++;
// while disabled, printout the duration of current disabled mode in seconds
if (GetClock() > printSec) {
// Move the cursor back to the previous line and clear it.
printf("\x1b1A\x1b2K");
printf("Disabled seconds: %d
", printSec - startSec);
printSec++;
}
}
void AutonomousPeriodic(void) {
m_autoPeriodicLoops++;
DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name);
// initialize position and destination variables
// position settings range from -1 to 1
// setServoPositions is a wrapper that handles the conversion to range for servo
horizontalDestination = 0.0; // final destination range -1.0 to +1.0
verticalDestination = 0.0;
// initialize pan variables
// incremental tasking toward dest (-1.0 to 1.0)
float incrementH, incrementV;
// pan needs a 1-up number for each call
int panIncrement = 0;
// current position range -1.0 to +1.0
horizontalPosition = RangeToNormalized(horizontalServo->Get(),1);
verticalPosition = RangeToNormalized(verticalServo->Get(),1);
// set servos to start at center position
setServoPositions(horizontalDestination, verticalDestination);
// for controlling loop execution time
float loopTime = 0.1;
//float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
// search variables
bool foundColor = 0;
double savedImageTimestamp = 0.0;
bool staleImage = false;
while( IsAutonomous() )
{
//GetWatchdog().Feed(); // turn watchdog off while debugging
// calculate gimbal position based on colors found
if ( FindTwoColors(td1, td2, ABOVE, &par1, &par2) ){
//PrintReport(&par2);
foundColor = true;
// reset pan
panIncrement = 0;
if (par1.imageTimestamp == savedImageTimestamp) {
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
DPRINTF(LOG_DEBUG, "STALE IMAGE");
} else {
// The target was recognized
// save the timestamp
staleImage = false;
savedImageTimestamp = par1.imageTimestamp;
DPRINTF(LOG_DEBUG,"image timetamp: %lf", savedImageTimestamp);
// Here is where your game-specific code goes
// when you recognize the target p
// get center of target
// Average the color two particles to get center x & y of combined target
horizontalDestination = (par1.center_mass_x_normalized + par2.center_mass_x_normalized) / 2;
verticalDestination = (par1.center_mass_y_normalized + par2.center_mass_y_normalized) / 2;
}
} else { // need to pan
foundColor = false;
}
if(foundColor && !staleImage) {
/* Move the servo a bit each loop toward the destination.
* Alternative ways to task servos are to move immediately vs.
* incrementally toward the final destination. Incremental method
* reduces the need for calibration of the servo movement while
* moving toward the target.
*/
incrementH = horizontalDestination - horizontalPosition;
// you may need to reverse this based on your vertical servo installation
//incrementV = verticalPosition - verticalDestination;
incrementV = verticalDestination - verticalPosition;
adjustServoPositions( incrementH, incrementV );
ShowActivity ("** %s & %s found: Servo: x: %f y: %f ** ",
td1.name, td2.name, horizontalDestination, verticalDestination);
} else { //if (!staleImage) { // new image, but didn't find two colors
// adjust sine wave for panning based on last movement direction
if(horizontalDestination > 0.0) { sinStart = PI/2.0; }
else { sinStart = -PI/2.0; }
/* pan to find color after a short wait to settle servos
* panning must start directly after panInit or timing will be off */
if (panIncrement == 3) {
panInit(8.0); // number of seconds for a pan
}
else if (panIncrement > 3) {
panForTarget(horizontalServo, sinStart);
/* Vertical action: In case the vertical servo is pointed off center,
* center the vertical after several loops searching */
if (panIncrement == 20) { verticalServo->Set( 0.5 ); }
}
panIncrement++;
ShowActivity ("** %s and %s not found ", td1.name, td2.name);
} // end if found color
// sleep to keep loop at constant rate
// this helps keep pan consistant
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if ( loopTime > ElapsedTime(lastTime) ) {
Wait( loopTime - ElapsedTime(lastTime) ); // seconds
}
} // end while
DPRINTF(LOG_DEBUG, "end Autonomous");
ShowActivity ("Autonomous end ");
/* the below code (if uncommented) would drive the robot forward at half speed
* for two seconds. This code is provided as an example of how to drive the
* robot in autonomous mode, but is not enabled in the default code in order
* to prevent an unsuspecting team from having their robot drive autonomously!
*/
/* below code commented out for safety
if (m_autoPeriodicLoops == 1) {
// When on the first periodic loop in autonomous mode, start driving forwards at half speed
m_robotDrive->Drive(0.5, 0.0); // drive forwards at half speed
}
if (m_autoPeriodicLoops == (2 * GetLoopsPerSec())) {
// After 2 seconds, stop the robot
m_robotDrive->Drive(0.0, 0.0); // stop robot
}
*/
} // end autonomous
void TeleopPeriodic(void) {
// feed the user watchdog at every period when in autonomous
GetWatchdog().Feed();
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
// Encoder printf
int count1 = encoderMotor1->Get();
int count2 = encoderMotor2->Get();
printf("Encoder Motor 1: %g
", count1);
printf("Encoder Motor 2: %g
", count2);
double distance1 = encoderMotor1->GetDistance();
double distance2 = encoderMotor1->GetDistance();
printf("EM1 Distance: %g
", distance1);
printf("EM2 Distance: %g
", distance2);
// Accelerometer printf
//printf("Accelerometer
", accl->GetAcceleration());
/*
* No longer needed since periodic loops are now synchronized with incoming packets.
if (m_ds->GetPacketNumber() != m_priorPacketNumber) {
*/
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received
// put Driver Station-dependent code here
// determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick
m_robotDrive->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
m_Shooter->ArcadeDrive(m_leftStick); // Shooter with arcade style
if (m_driveMode != ARCADE_DRIVE) {
// if newly entered arcade drive, print out a message
printf("Arcade Drive
");
m_driveMode = ARCADE_DRIVE;
}
// If you press Button 4 the relay goes forward
if (m_leftStick->GetRawButton(5) == 1 && flipperdoorright->Get() > 0 ) {
flipperdoor->Set(Relay::kForward);
//delay(10);
} else if (m_leftStick->GetRawButton(4) == 1 && flipperdoorleft->Get() > 0 ){
flipperdoor->Set(Relay::kReverse);
//delay(10);
}
else{
flipperdoor->Set(Relay::kOff);
}
// If you press button 5 the relay goes backwards
//if (m_leftStick->GetRawButton(5) == 1) {
// flipperdoor->Set(Relay::kReverse);
//} else {
// flipperdoor->Set(Relay::kOff);
//}
// If you press button 3 the joystick controls the lift instead of the shooter
if (m_leftStick->GetRawButton(3) == 1) {
m_Lift->ArcadeDrive(m_leftStick);
} else {
m_Shooter->ArcadeDrive(m_leftStick);
}
/*
} // if (m_ds->GetPacketNumber()...
*/
} // TeleopPeriodic(void)
/********************************** Continuous Routines *************************************/
/*
* These routines are not used in this demonstration robot
*
*
void DisabledContinuous(void) {
}
void AutonomousContinuous(void) {
}
void TeleopContinuous(void) {
}
*/
};
START_ROBOT_CLASS(BuiltinDefaultCode);
