/**
* Constructor for this robot subclass.
* Create an instance of a RobotDrive with left and right motors
* plugged into PWM ports 1 and 2 on the first digital module. The two
* servos are PWM ports 3 and 4.
*/
VisionServoDemo(void)
{
ds = DriverStation::GetInstance();
myRobot = new RobotDrive(1, 2, 0.5); // robot will use PWM 1-2 for drive motors
rightStick = new Joystick(1); // create the joysticks
leftStick = new Joystick(2);
horizontalServo = new Servo(3); // create horizontal servo
verticalServo = new Servo(4); // create vertical servo
servoDeadband = 0.01; // move if > this amount
sinStart = 0.0; // control whether to start panning up or down
m_pDashboard = &(m_ds->GetDashboardPacker());
/* set up debug output:
* DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY,
* DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE
*/
SetDebugFlag(DEBUG_FILE_ONLY);
/* start the CameraTask */
if (StartCameraTask(20, 0, k160x120, ROT_180) == -1)
{
DPRINTF(LOG_ERROR,
"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()));
}
m_pVideoServer = new PCVideoServer;
/* allow writing to vxWorks target */
Priv_SetWriteFileAllowed(1);
/* stop the watchdog if debugging */
GetWatchdog().SetEnabled(false);
}
/**
* Constructor for this robot subclass.
* Create an instance of a RobotDrive with left and right motors plugged into PWM
* ports 1 and 2 on the first digital module. The two servos are PWM ports 3 and 4.
* Tested with camera settings White Balance: Flurorescent1, Brightness 40, Exposure Auto
*/
TwoColorDemo(void)
{
ds = DriverStation::GetInstance();
myRobot = new RobotDrive(1, 2, 0.5); // robot will use PWM 1-2 for drive motors
rightStick = new Joystick(1); // create the joysticks
leftStick = new Joystick(2);
// 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(&par, 0, sizeof(par)); // 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);
}
TargetDetector::TargetDetector(void)
{
ds = DriverStation::GetInstance();
framesPerSecond = 15; // number of camera frames to get per second
cameraRotation = ROT_0; // input parameter for camera orientation
memset(&par,0,sizeof(par)); // 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_180) == -1)
{
DPRINTF( LOG_ERROR,"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()) );
}
Wait(1.0);
StartImageAcquisition();
/* allow writing to vxWorks target */
Priv_SetWriteFileAllowed(1);
savedImageTimestamp = 0.0;
}
Robot980::Robot980()
: m_tcTraction(TC_OFF)
, m_psdLeft(NULL)
, m_psdRight(NULL)
// left and right drive motors
, m_pscLeft(new ReversableJaguar(SLOT_PWM_LEFT, CHAN_PWM_LEFT, true))
, m_pscRight(new Jaguar(SLOT_PWM_RIGHT,CHAN_PWM_RIGHT))
// lower and upper drive belts
, m_pscLowerBelt(new Jaguar(SLOT_PWM_LOWER, CHAN_PWM_LOWER))
, m_pscUpperBelt(new Jaguar(SLOT_PWM_UPPER, CHAN_PWM_UPPER))
// ball release
, m_pscFlap(new Jaguar(SLOT_PWM_FLAP, CHAN_PWM_FLAP))
// sensors
, m_pGyro(new Gyro(SLOT_GYRO, CHAN_GYRO))
// encoders on the drive wheels
, m_pEncDrvLeft(new Encoder(SLOT_ENC_DRV_LEFT,
CHAN_ENC_DRV_LEFT_A,
SLOT_ENC_DRV_LEFT,
CHAN_ENC_DRV_LEFT_B,
false, ENC_SCALE))
, m_pEncDrvRight(new Encoder(SLOT_ENC_DRV_RIGHT,
CHAN_ENC_DRV_RIGHT_A,
SLOT_ENC_DRV_RIGHT,
CHAN_ENC_DRV_RIGHT_B,
false, ENC_SCALE))
// encoders on the follow wheels
, m_pEncFollowLeft(new Encoder(SLOT_ENC_FOLLOW_LEFT,
CHAN_ENC_FOLLOW_LEFT_A,
SLOT_ENC_FOLLOW_LEFT,
CHAN_ENC_FOLLOW_LEFT_B,
false, ENC_SCALE))
, m_pEncFollowRight(new Encoder(SLOT_ENC_FOLLOW_RIGHT,
CHAN_ENC_FOLLOW_RIGHT_A,
SLOT_ENC_FOLLOW_RIGHT,
CHAN_ENC_FOLLOW_RIGHT_B,
false, ENC_SCALE))
// Timer used for debugging
, m_pTimer(new Timer)
, m_pSrvPan(new Servo(CAMERA_SLOT_PAN, CAMERA_CHAN_PAN))
, m_pSrvTilt(new Servo(CAMERA_SLOT_TILT, CAMERA_CHAN_TILT))
, m_pVideoServer(NULL)
// , m_pCamControlLoop(new Notifier(Robot980::CallCamUpdate, this))
, m_pCamControlLoop(NULL)
, m_dSavedImageTimestamp(0.0)
, m_trackColor(DriverStation::kInvalid)
{
// pi * diameter * gear ratio / encoder ticks / in/ft
// 48:32 = 1.5 on drive wheels
m_pEncDrvLeft->SetDistancePerPulse(M_PI * 6 * GEAR_RATIO / 250 / 12);
m_pEncDrvRight->SetDistancePerPulse(M_PI * 6 * GEAR_RATIO / 250 / 12);
m_pEncFollowLeft->SetDistancePerPulse(M_PI * 60/25.4 / 250 / 12);
m_pEncFollowRight->SetDistancePerPulse(M_PI * 60/25.4 / 250 / 12);
m_pEncDrvLeft->Start();
m_pEncDrvRight->Start();
m_pEncFollowLeft->Start();
m_pEncFollowRight->Start();
m_pTimer->Reset();
m_pTimer->Start();
#define SD_TIME 0.020
// // "Smart Drive" handles PID, slipping, etc
// m_psdLeft = new SmartDrive(SD_ID_LEFT,
// 0.6, 0.1, // velocity PID constants
// 0.1, 0.1, // correction PID constants
// 0.2, 0.1, // acceleration PID constants
// true,
// m_pscLeft, m_pEncDrvLeft, m_pEncFollowLeft,
// SD_TIME);
// Wait(SD_TIME / 2);
// m_psdRight = new SmartDrive(SD_ID_RIGHT,
// 0.6, 0.1, // velocity PID constants
// 0.1, 0.1, // correction PID constants
// 0.2, 0.1, // acceleration PID constants
// true,
// m_pscRight, m_pEncDrvRight, m_pEncFollowRight,
// SD_TIME);
// "Smart Drive" handles PID, slipping, etc
m_psdLeft = new SmartDrive(SD_ID_LEFT,
0.6, 0.1, // velocity PID constants
1.0, 0.04, // correction PID constants
1.0, 0.0, false, // acceleration PID constants
m_pscLeft, m_pEncDrvLeft, m_pEncFollowLeft,
SD_TIME);
Wait(SD_TIME / 2);
m_psdRight = new SmartDrive(SD_ID_RIGHT,
0.6, 0.1, // velocity PID constants
1.0, 0.04, // correction PID constants
1.0, 0.0, false, // acceleration PID constants
m_pscRight, m_pEncDrvRight, m_pEncFollowRight,
SD_TIME);
ParticleAnalysisReport par1, par2; // particle analysis reports
memset(&par1, 0, sizeof(ParticleAnalysisReport));
memset(&par2, 0, sizeof(ParticleAnalysisReport));
/* image data for tracking - override default parameters if needed */
/* recommend making PINK the first color because GREEN is more
* susceptible to hue variations due to lighting type so may
* result in false positives */
// PINK
sprintf(m_tdPink.name, "PINK");
m_tdPink.hue.minValue = 220;
m_tdPink.hue.maxValue = 255;
m_tdPink.saturation.minValue = 75;
m_tdPink.saturation.maxValue = 255;
m_tdPink.luminance.minValue = 85;
m_tdPink.luminance.maxValue = 255;
// GREEN
sprintf(m_tdGreen.name, "GREEN");
m_tdGreen.hue.minValue = 55;
m_tdGreen.hue.maxValue = 125;
m_tdGreen.saturation.minValue = 58;
m_tdGreen.saturation.maxValue = 255;
m_tdGreen.luminance.minValue = 92;
m_tdGreen.luminance.maxValue = 255;
printf("Robot980 constructor\n");
m_trailerInfo.color = DriverStation::kInvalid;
EnableTractionControl(TC_SMART_2);
Wait(0.010);
EnableTractionControl(TC_LOWPASS);
/* start the CameraTask */
StartCameraTask(CAMERA_FPS, CAMERA_COMPRESSION, CAMERA_RESOLUTION,
CAMERA_ROTATION);
// m_pVideoServer = new PCVideoServer;
m_pCamControlLoop->StartPeriodic((double)1.0 / (double)CAMERA_FPS);
// tell SensorBase about us
AddToSingletonList();
}
