| dboisvert |
02-14-2009 07:22 PM |
System: Watchdog problems
Recently my team flashed the newest Driver Station software found here
Ever since we did that & rewired some of the electrical box we have been getting a "System: WatchDog" and I went to here to debug it.
Competition port dongle is in and set to enabled
Everything is networked properly ( I pinged each IP to make sure)
The cRIO has FRC_2009_V11.zip imaged to it, Workbench is updated to 3.0.1718
Lastly the code I was trying to run on it is as follows
Code:
#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\n");
// 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\n");
}
/********************************** Init Routines *************************************/
void RobotInit(void) {
printf("RobotInit() completed.\n");
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("\x1b[2B");
}
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("\x1b[1A\x1b[2K");
printf("Disabled seconds: %d\r\n", 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\n", count1);
printf("Encoder Motor 2: %g\n", count2);
double distance1 = encoderMotor1->GetDistance();
double distance2 = encoderMotor1->GetDistance();
printf("EM1 Distance: %g\n", distance1);
printf("EM2 Distance: %g\n", distance2);
// Accelerometer printf
//printf("Accelerometer\n", 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\n");
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);
As always any help is much appreciated, especially at crunch time! |
