/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
compressor->Start();
GetWatchdog().SetExpiration(0.5);
bool valve_state = false;
while (IsOperatorControl())
{
motor->Set(stick->GetY());
if (stick->GetRawButton(1) && !valve_state)
{
valve->Set(true);
valve_state = true;
}
if (!stick->GetRawButton(1) && valve_state)
{
valve->Set(false);
valve_state = false;
}
// Update driver station
//dds->sendIOPortData(valve);
GetWatchdog().Feed();
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
AnalogTrigger trig1(2);
trig1.SetLimitsVoltage(1.5, 2.5);
AnalogTrigger trig2(3);
trig2.SetLimitsVoltage(1.5, 2.5);
Encoder encoder(
trig1.CreateOutput(AnalogTriggerOutput::kRisingPulse),
trig2.CreateOutput(AnalogTriggerOutput::kRisingPulse),
false, Encoder::k2X);
double tm = GetTime();
GetWatchdog().SetEnabled(true);
while (IsOperatorControl())
{
GetWatchdog().Feed();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
if (GetTime() - tm > 0.25)
{
printf("Encoder: %d\r", encoder.Get());
tm = GetTime();
}
Wait(0.005); // wait for a motor update time
}
}
//robot turns to desired position with a deadband of 2 degrees in each direction
bool GyroTurn (float desiredTurnAngle, float turnSpeed)
{
bool turning = true;
float myAngle = gyro->GetAngle();
//normalizes angle from gyro to [-180,180) with zero as straight ahead
while(myAngle >= 180)
{
GetWatchdog().Feed();
myAngle = myAngle - 360;
}
while(myAngle < -180)
{
GetWatchdog().Feed();
myAngle = myAngle + 360;
}
if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit
{
myRobot->Drive(turnSpeed, -turnSpeed); //(right,left)
}
if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit
{
myRobot->Drive(-turnSpeed, turnSpeed); //(right,left)
}
else
{
myRobot->Drive(0, 0);
turning = false;
}
return turning;
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
while (IsOperatorControl())
{
GetWatchdog().Feed();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
}
}
Spike::Spike(void)
{
GetWatchdog().SetExpiration(10.0);//set up watchdog
GetWatchdog().SetEnabled(true);
Drive = new RobotDrive(LEFT_DRV_PWM,RIGHT_DRV_PWM);
rightjoy = new Joystick(1);
leftjoy = new Joystick(2);
}
void DashBoardInput() {
int i = 0;
GetWatchdog().SetEnabled(true);
while (IsAutonomous() && IsEnabled()) {
i++;
GetWatchdog().Feed();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i",
driverStation->GetAnalogIn(1), i);
dsLCD->UpdateLCD();
}
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
{
myRobot->Drive(0.5, 0.0); // drive forwards half speed
Wait(2000); // for 2 seconds
myRobot->Drive(0.0, 0.0); // stop robot
}
GetWatchdog().SetEnabled(true);
}
/**
* 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);
}
/**
* Runs the motors with arcade steering.
*/
void RobotMain(void)
{
GetWatchdog().SetEnabled(false);
while (true)
{
GetWatchdog().Feed();
sendIOPortData();
sendVisionData();
Wait(1.0);
}
}
/****************************************
* MainRobot: (The constructor)
* Mandatory method.
* TODO:
* - Tweak anything related to the scissor lift - verify values.
* - Find out how to configure Victor.
*/
MainRobot(void):
// Below: The constructor needs to know which port connects to which
// motor so it can control the Jaguars correctly.
// See constants at top.
robotDrive(LEFT_FRONT_MOTOR_PORT, LEFT_REAR_MOTOR_PORT,
RIGHT_FRONT_MOTOR_PORT, RIGHT_REAR_MOTOR_PORT)
{
SmartDashboard::init();
GetWatchdog().SetExpiration(0.1); // In seconds.
stick1 = new Joystick(MOVE_JOYSTICK_USB); // Joystick 1
stick2 = new Joystick(LIFT_JOYSTICK_USB); // Joystick 2
minibot = new MinibotDeployment (
MINIBOT_DEPLOY_PORT,
MINIBOT_DEPLOYED_DIO,
MINIBOT_RETRACTED_DIO);
lineSensors = new LineSensors (
LEFT_CAMERA_PORT,
MIDDLE_CAMERA_PORT,
RIGHT_CAMERA_PORT);
lift = new LiftController (
LIFT_MOTOR_PORT,
HIGH_LIFT_DIO,
LOW_LIFT_DIO);
lift->initButtons(
kJSButton_2, // Botton top button
kJSButton_4, // Left top button
kJSButton_3, // Center top button
kJSButton_5); // Right top button
// The wiring was inverted on the left motors, so the below
// is necessary.
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
isFastSpeedOn = false;
isSafetyModeOn = true;
isLiftHigh = false;
// isSafetyModeOn: Controlled by the driver -- should only have to
// choose once.
// isLiftHigh: Controlled by the program -- turns true only
// when height is too high, otherwise turns false.
isDoingPreset = false;
GetWatchdog().SetEnabled(true);
UpdateDashboard("TestingTestingTestingTesting"
"TestingTestingTestingTestingTesting");
UpdateDashboard("Finished initializing.");
}
void OperatorControl(void)
{
GetWatchdog().SetExpiration(.1);
GetWatchdog().SetEnabled(true);
GetWatchdog().Feed();
while (IsOperatorControl())
{
GetWatchdog().Feed();
UpdateDash();
Wait(.001f);
}
}
void RobotBeta1::resetGyro(void) {
DBG("Enter\n");
float angle;
do {
gyro->Reset();
angle = gyro->GetAngle();
DBG("calibrate angle %f\r", angle);
GetWatchdog().Feed();
Wait(0.1);
GetWatchdog().Feed();
} while (((int)angle) != 0);
DBG("\nExit\n");
}
/**
* Run the closed loop position controller on the Jag.
*/
void OperatorControl()
{
printf("In OperatorControl\n");
GetWatchdog().SetEnabled(true);
while (IsOperatorControl() && !IsDisabled())
{
GetWatchdog().Feed();
// Set the desired setpoint
speedJag.Set(stick.GetAxis(axis) * 150.0);
UpdateDashboardStatus();
Wait(0.05);
}
}
Robot2014(void){
GetWatchdog().SetExpiration(1);
GetWatchdog().SetEnabled(false);
drivePad = new Joystick(DRIVEPAD);
gamePad = new Joystick(GAMEPAD);
driveTrain = new MecanumDrive();
ballShooter = new Shooter();
pickupBall = new BallPickup();
//vision = new VisionSystem();
autonTimer = new Timer();
}
void Test(void)
{
compressor->Start();
myRobot.SetSafetyEnabled(true);
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
while(IsTest())
{
GetWatchdog().Feed();
Wait(0.1);
}
}
void OperatorControl(void) {
XboxController *xbox = XboxController::getInstance();
bool isEndGame = false;
GetWatchdog().SetEnabled(true);
_driveControl.initialize();
//_poleVaultControl.initialize();
shooterControl.InitializeOperator();
while (IsEnabled() && IsOperatorControl()) {
GetWatchdog().Feed();
dsLCD->Clear();
if (xbox->isEndGame()) {
isEndGame = !isEndGame;
}
if (!isEndGame) {
shooterControl.Run();
//_poleVaultControl.act();
_driveControl.act();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal");
led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff);
led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff);
}
else {
shooterControl.RunEndGame();
//_poleVaultControl.actEndGame();
_driveControl.actEndGame();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game");
flashCount--;
if (flashCount<=0){
isFlashing = !isFlashing;
flashCount=FLASHTIME;
}
led0->Set((isFlashing)?Relay::kOn: Relay::kOff);
led1->Set((isFlashing)?Relay::kOn: Relay::kOff);
}
// dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount);
dsLCD->UpdateLCD();
Wait(WAIT_TIME); // wait for a motor update time
}
GetWatchdog().SetEnabled(false);
}
void AutonomousContinuous(void) {
printf("Running in autonomous continuous...\n");
GetWatchdog().Feed();
if (kicker->HasBall())
{
//We have a ball, thus stop moving and kick the ball
drivetrain->Drive(0.0, 0.0);
kicker->SetKickerMode(KICKER_MODE_KICK);
} else {
//We do not have a ball
if (kicker->IsKickerInPosition())
{
//Move forward!
drivetrain->ArcadeDrive(autonomousForwardPower, 0.0);
} else {
//If not in position, wait for it to be there...
drivetrain->ArcadeDrive(0.0, 0.0);
kicker->SetKickerMode(KICKER_MODE_ARM);
}
}
//Run the kicker
kicker->Act();
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
myRobot.Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
/********************************* INIT FUNCTIONS *********************************/
void RobotInit(void) {
printf("Robot initializing...\n");
GetWatchdog().Feed();
printf("Robot initialization complete.\n");
}
void tune_jog(void)
{
GetWatchdog().Feed();
static volatile float time;
static volatile float pos;
if (t_axis > na)
return;
if (t_jog_time[t_axis] < .1)
return;
time += 0.01;
if ((time >= 0) && ( time < t_jog_time[t_axis] ))
return;
time = 0;
if (pos == 12) pos = 24;
else pos = 12;
set_position(t_axis, pos, 1, -.35);
/*
if (pot_pos[t_axis] < t_pos_dn[t_axis])
set_position (t_axis, t_pos_up[t_axis], t_lim_up[t_axis],t_lim_dn[t_axis]);
if (pot_pos[t_axis] > t_pos_up[t_axis])
set_position (t_axis, t_pos_dn[t_axis], t_lim_up[t_axis],t_lim_dn[t_axis]);
*/
}
void AutonomousStateMachine() {
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
enum AutoState {
AutoDrive, AutoSetup, AutoShoot
};
AutoState autoState;
autoState = AutoDrive;
bool feederState = false;
bool hasFired = false;
Timer feeder;
while (IsAutonomous() && IsEnabled()) {
GetWatchdog().Feed();
switch (autoState) {//Start of Case Machine
case AutoDrive://Drives the robot to set Destination
bool atDestination = driveControl.autoPIDDrive2();
if (atDestination) {//If at Destination, Transition to Shooting Setup
autoState = AutoSetup;
}
break;
case AutoSetup://Starts the ballgrabber motors to place the ball and extends ballgrabber
if (!pneumaticsControl->ballGrabberIsExtended()) {//extends ballgrabber if not already extended
pneumaticsControl->ballGrabberExtend();
}
if (!feederState) {//Started the feeder timer once
feederState = true;
feeder.Start();
autoState = AutoShoot;
}
break;
case AutoShoot://Shoots the ball
if (feeder.Get() < 2.0) {//Runs ballgrabber for 2.0 Seconds at most
shooterControl->feed(true);
} else if (feeder.Get() >= 2.0) {//Transition to Shooting
shooterControl->feed(false);
feeder.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !hasFired) {
shooterControl->autoShoot();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//Returns true only after shoot is done firing
hasFired = true;
}
} else if (hasFired) {//runs only after shoot is done
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"AutoMode Finished");
}
break;
}
dsLCD->UpdateLCD();
}
}
/**
* 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);
}
void Michael1::Autonomous(void)
{
printf("\n\n\tStart Autonomous:\n\n");
GetWatchdog().SetEnabled(false);
ariels_light->Set(1);
while (IsAutonomous())
{
Wait(0.1); //important
dt->SmoothTankDrive(left_stick, right_stick);
//dt->UpdateSlip();
//dt->UpdateSlip(); //calling slipControl(true) should spawn a task which does this.
//printf("Encoder: %f, ", dt->encoder_left->GetDistance());
//printf("Gyro: %f, ", dt->gyro->GetAngle());
//printf("Accel: %f", dt->accel->GetAcceleration());
//printf("\n\n");s
/*Wait(.1);
if(cam->FindTargets()){
ariels_light->Set(1);
} else {
ariels_light->Set(0);
}
*/
}
}
void DisabledPeriodic(void) {
GetWatchdog().Feed();
disabledPeriodicLoops++;
if ((disabledPeriodicLoops % 4) == 0) {
tiltA = (90.0 - (180.0 * (tiltEncoder->GetAverageVoltage() - 0.5) / 4.0));
lThrottleValue = leftStick->GetThrottle();
shootDelay = (4 - (((rightStick->GetThrottle()) + 1) * 2)); // Allows values from 0 - 4 instead of -1 to 1
Write2LCD();
}
if (lThrottleValue > 0) {
autonMode = 3;
}
else if (lThrottleValue < 0) {
autonMode = 2;
}
}
/**
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis.
*/
void OperatorControl(void)
{
Victor armMotor(5); // create arm motor instance
while (IsOperatorControl())
{
GetWatchdog().Feed();
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
// Control the movement of the arm using the joystick
// Use the "Y" value of the arm joystick to control the movement of the arm
float armStickDirection = armStick->GetY();
// if at a limit and telling the arm to move past the limit, don't drive the motor
if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
armStickDirection = 0;
} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
armStickDirection = 0;
}
// Set the motor value
armMotor.Set(armStickDirection);
}
}
void OperatorControl(void) {
GetWatchdog().SetEnabled(true);
dsLCD->Clear();
dsLCD->UpdateLCD();
driveControl.initialize();
pneumaticsControl->initialize();
shooterControl->initialize();
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed();
driveControl.run();
shooterControl->run();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
pneumaticsControl->disable();
}
/****************************************
* UpdateDashboard:
* Input = none
* Output = Safety mode
* Watchdog state
* Robot Speed
* System state (Autonomous or Teleoperated?)
* Robot state (Enabled or Disabled?)
* Timer
* Minibot alert
* Dependent on the 'SmartDashboard' class from the WPI library.
* TODO:
* - Test to see if this works.
*/
void UpdateDashboard(void)
{
// Setup here (default values set):
const char *watchdogCheck;
if (GetWatchdog().IsAlive()) {
watchdogCheck = GetWatchdog().GetEnabled() ? "Enabled" : "DISABLED";
} else {
watchdogCheck = "DEAD";
}
const char *systemState;
if (IsOperatorControl()) {
systemState = "Teleoperate";
} else if (IsAutonomous()) {
systemState = "Autonomous";
} else {
systemState = "???";
}
const char *minibotStatus;
float currentTime = timer.Get();
if (currentTime >= (GAMEPLAY_TIME - 15)) {
minibotStatus = "Get Ready";
if (currentTime >= (GAMEPLAY_TIME - 10)) {
minibotStatus = "DEPLOY";
}
} else {
minibotStatus = "...";
}
// Safety info
SmartDashboard::Log(isSafetyModeOn ? "ENABLED" : "Disabled", "Safety mode: ");
SmartDashboard::Log(watchdogCheck, "Watchdog State: ");
// Robot actions
SmartDashboard::Log(isFastSpeedOn ? "Fast" : "Normal", "Speed: ");
SmartDashboard::Log(lift->getCurrentHeight(), "Current Lift Height: ");
// Info about the field state
SmartDashboard::Log(systemState, "System State: ");
SmartDashboard::Log(IsEnabled() ? "Enabled" : "DISABLED", "Robot State: ");
// Info about time
SmartDashboard::Log(minibotStatus, "MINIBOT ALERT: ");
}
void Metrobot::RobotInit(){
drive = new Drive( flMotor, blMotor, frMotor, brMotor,
flEncoder, blEncoder, frEncoder, brEncoder, gyro );
drive->SetInvertedMotors( false, false, false, false );
vision = Vision::GetInstance();
ds = DriverStationLCD::GetInstance();
autonScript = NO_SCRIPT;
autonStep = 0;
GetWatchdog().SetExpiration( 0.1 );
GetWatchdog().SetEnabled( true );
}
CB1(void) {
frontLeft = new Victor(1); //New Motors
backLeft = new Victor(2);
frontRight = new Victor(3);
backRight = new Victor(4);
shooterFront = new Victor(5);
shooterBack = new Victor(6);
tiltShooter = new Victor(7);
drive = new RobotDrive(frontLeft, backLeft, frontRight, backRight); //Drive Train
discPush = new DoubleSolenoid(1,2); //Pneumatics
pumpAir = new Compressor(14,8);
ds = DriverStation::GetInstance(); //Driver Station
dsLCD = DriverStationLCD::GetInstance();
priorPacketNumber = 0;
dsPacketsReceivedInCurrentSecond = 0;
rightEncoder = new AugmentedEncoder(4, 5, d_p_r / t_p_r, false); //Encoders
leftEncoder = new AugmentedEncoder(6, 7, d_p_r / t_p_r, true);
shooterEncoder = new AugmentedEncoder(8, 9, d_p_r / t_p_r, false);
tiltEncoder = new AnalogChannel(1, 1);
autotimer = new Timer(); //Timers
blinktimer = new Timer();
teletimer = new Timer();
rightStick = new Joystick(1); //Controls
leftStick = new Joystick(2);
pilotPad = new LogitechGamepad(3);
shooterState = SHOOTEROFF; //Values
pistonState = PISTONO;
autoPeriodicLoops = 0;
disabledPeriodicLoops = 0;
telePeriodicLoops = 0;
rightD = 0.0;
leftD = 0.0;
rightV = 0.0;
leftV = 0.0;
shooterV = 0.0;
pumpAir->Start(); //Compressor Start
leftEncoder->Start(); //Encoders Start
rightEncoder->Start();
shooterEncoder->Start();
autonMode = 2;
lThrottleValue = 0;
shootDelay = 0;
GetWatchdog().SetExpiration(50); //50ms Timeout
}
RobotDemo(void)
{
///constructs
game = false;//set to true or false before use- true for auton & tele-op, false for just 1
cd = new CustomDrive(NUM_JAGS);
closed = new Solenoid(8, 1);//true if closed
open = new Solenoid(8, 2);
stick = new Joystick(2);// arm controll
controller = new Joystick(1);// base
manEnc = new Encoder(4,5);
reset = false;//if button
manJag = new Jaguar(6);//cons manip Jag
ShoulderJag = new Jaguar(5);
minibot = new DoubleSolenoid(3, 4);cmp = new Compressor(DOC7_RELAY_PORT, DOC7_COMPRESSOR_PORT);
track = new Tracking(DOC7_LEFT_LINE_PORT, DOC7_CENTER_LINE_PORT, DOC7_RIGHT_LINE_PORT);
red_white = new Relay(DOC7_RED_WHITE_SPIKE);
red_white->SetDirection(Relay::kBothDirections);
red_white->Set(Relay::kOff);
blue = new Relay(DOC7_BLUE_SPIKE);
blue->SetDirection(Relay::kBothDirections);
blue->Set(Relay::kOff);
GetWatchdog().Kill();
};
