void Machine :: TeleopInit()
{
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Ben is here");
lcd->UpdateLCD();
drive.enableVoltageControl();
//drive.enableSpeedControl();
}
RobotDemo(void):
myRobot(1, 2), // these must be initialized in the same order
stick(1) // as they are declared above.
{
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "XboxController2");
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
}
Hohenheim(void) {
driverStation = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
pneumaticsControl = PneumaticsControl::getInstance();
shooterControl = ShooterControl::getInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hohenheim 2014 V 3.1");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
}
// Code to be run during the remaining 2:20 of the match (after Autonomous())
//
// OperatorControl
// * Calls all the above methods
void OperatorControl()
{
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(false);
Timer clock;
int sanity = 0;
int bigSanity = 0;
loading = false;
loaded = winchSwitch.Get();
RegisterButtons();
gamepad.Update();
leftStick.Update();
compressor.Start();
while (IsOperatorControl() && IsEnabled())
{
clock.Start();
HandleDriverInputs();
HandleShooter();
HandleArm();
// HandleEject();
while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
clock.Reset();
sanity++;
if (sanity >= 100)
{
bigSanity++;
sanity = 0;
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
}
gamepad.Update();
leftStick.Update();
dsLCD->UpdateLCD();
}
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
void
DriverMessages::SendTextLines()
{
DriverStationLCD *lcd =DriverStationLCD::GetInstance();
for(int i = 0; i < 3;i++){
lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)lineText[i]);
}
lcd->UpdateLCD();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void) {
while (IsOperatorControl()) {
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f",
signal.GetVoltage());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "CVoltage: %f",
signalControlVoltage.GetVoltage());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
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();
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void DriveThenShootAuto() {
//initizlizes all parts of robot
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
bool destinationPrevious = false;
bool autoShot = false; //true if autoShoot
//creates a timer for the ball grabber motors
Timer feeding;
bool started = false;
while (IsAutonomous() && IsEnabled()) {
GetWatchdog().Feed();
//drives forward to shooting point
bool atDestination = destinationPrevious || driveControl.autoPIDDrive2(); //autoDrive returns true when robot reached it's goal
if (atDestination) {
// The robot has reached the destination on the floor and is now ready to open and shoot
if (!started) {
started = true;
destinationPrevious = true;
//starts feeding-timer controls feeder motors so the ball doesn't get stuck
feeding.Start();
}
pneumaticsControl->ballGrabberExtend();
//waits for feeding to be done
if (feeding.Get() < 2.0) {//3.0 was
shooterControl->feed(true);
} else if (feeding.Get() >= 2.0) { // 3.0 was
shooterControl->feed(false);
feeding.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !autoShot) {
shooterControl->autoShoot();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//works only after shoot is done firing
autoShot = true;
}
} else if (autoShot) {//runs only after shoot is done
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"AutoMode Finished");
}
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Feeder Time: %f",
feeding.Get());
dsLCD->UpdateLCD();
}
}
SebastianRobot(void) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Sebastian V24.2");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
led0 = new Relay(2);
led1 = new Relay(3);
flashCount = 0;
led0->Set(Relay::kOff);
led1->Set(Relay::kOff);
isFlashing=false;
}
RobotDemo(void)
{
motor = new Jaguar(9);
stick = new Joystick(1);
compressor = new Compressor(1, 1);
valve = new Solenoid(8);
// Construct the dashboard sender object used to send hardware state
// to the driver station
// dds = new DashboardDataSender();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Plyboy test code: 6:46PM 1/2/2012");
dsLCD->UpdateLCD();
}
void Disabled()
{
while(IsDisabled())
{
LEDLight->Set(Relay::kForward);
rpi->Read();
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos());
lcd->UpdateLCD();
}
}
void Print ()
{
if (PrintTime.Get() > PRINT_TIME)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed());
lcd->UpdateLCD();
PrintTime.Reset();
PrintTime.Start();
}
}
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 AutonomousPeriodic(void) {
m_autoPeriodicLoops++;
#if 0
static int Clock=0;
bool correct = DriveStick->GetButton(Joystick::kTriggerButton);
bool Reset = DriveStick->GetButton (Joystick::kTopButton);
ds->PrintfLine(DriverStationLCD::kUser_Line1, "%s %s",
correct ? "correct on" : "correct off",
Reset ? "Reset": "No Reset");
//ds->PrintfLine(DriverStationLCD::kUser_Line6, "%d %c %c", Clock, correct? "C" : "c", Reset? "R" : "r");
if (Reset)
Clock=0, MyRobot.ResetCounters();
else
{
++Clock;
if(Clock<=100) MyRobot.DriveRobot(1.0,0.0, ds, correct); // drive forward
else if (Clock<=200) MyRobot.DriveRobot(-1.0,0.0, ds, correct); // stop
else if (Clock<=250) MyRobot.DriveRobot(-1.0,0.0, ds, correct); // drive back halfway
else if (Clock<=300) MyRobot.DriveRobot(1.0,0.0, ds, correct); // stop
else
{
// Real teleop mode: use the JoySticks to drive
MyRobot.DriveRobot(DriveStick->GetY(),(DriveStick->GetX()), ds);
}
}
#endif
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
HSLImage *Himage;
Threshold targetThreshold(247, 255, 60, 140, 10, 50);
BinaryImage *matchingPixels;
vector<ParticleAnalysisReport> *pReport;
//myRobot->SetSafetyEnabled(true);
Saftey->SetEnabled(false);
AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11");
mycam.WriteResolution(AxisCamera::kResolution_640x480);
mycam.WriteCompression(20);
mycam.WriteBrightness(25);
Wait(3.0);
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
float X[2];
float Y[2];
float Z[2];
while(IsOperatorControl())
{
X[1] = Stick1->GetX();
X[2] = Stick2->GetX();
Y[1] = Stick1->GetY();
Y[2] = Stick2->GetY();
Z[1] = Stick1->GetZ();
Z[2] = Stick2->GetZ();
Jaguar1->Set(Y[1]);
Jaguar2->Set(Y[2]);
Wait(0.005);
if (mycam.IsFreshImage())
{
Himage = mycam.GetImage();
matchingPixels = Himage->ThresholdHSL(targetThreshold);
pReport = matchingPixels->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < pReport->size(); i++)
{
printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y);
}
delete Himage;
delete matchingPixels;
delete pReport;
}
}
//myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick)
//Wait(0.005); // wait for a motor update time
}
RobotDemo(void):
gamepad(3)
{
dsLCD = DriverStationLCD::GetInstance();
// Output the program name and build date/time in the hope that this will help
// us catch cases where we are downloading a program other than the one
// we think we are downloading. Keep in mind that if this source file
// does not change (and you don't do a complete rebuild) the timestamp
// will not change.
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Menu");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
}
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();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
comp->Start();
while(IsOperatorControl())
{
if(wasenabled == false && IsEnabled() == true)
{
//arm->initialize();
wasenabled = true;
//Wait(.5);
}
wasenabled = IsEnabled();
d->go();
//r->Set((r->kForward));
//ds->PrintfLine(ds->kUser_Line1, "button 11: %d " ,stick->GetRawButton(11));
//ds->PrintfLine(ds->kUser_Line2, "button 2: %d", stick->GetRawButton(2));
//arm->pickup();
if(stick->GetRawButton(7))
{
armlock->Set(armlock->kForward);
}
else
{
armlock->Set(armlock->kReverse);
}
ds->PrintfLine(ds->kUser_Line2, "button 7: %d", stick->GetRawButton(7));
//arm->shoot();
ds->UpdateLCD();
}
}
void Run(CompressorInputs input) {
//cout << input.enable;
//compressor->Set(Relay::kOn);
//compressor->Set(input.enable ? Relay::kOn : Relay::kOff);
//cout << compressor->GetPressureSwitchValue();
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Pressure Switch: %x",
compressor->GetPressureSwitchValue());
}
FRC2994_2014():
leftFrontDrive(LEFT_FRONT_DRIVE_PWM),
leftRearDrive(LEFT_REAR_DRIVE_PWM),
rightFrontDrive(RIGHT_FRONT_DRIVE_PWM),
rightRearDrive(RIGHT_REAR_DRIVE_PWM),
robotDrive(&leftFrontDrive, &leftRearDrive, &rightFrontDrive, &rightRearDrive),
rightStick(RIGHT_DRIVE_STICK)
{
// Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE.
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " " __TIME__);
dsLCD->UpdateLCD();
robotDrive.SetExpiration(0.1);
}
RobotDemo(void):
leftDriveMotor(LEFT_DRIVE_PWM),
rightDriveMotor(RIGHT_DRIVE_PWM),
myRobot(&leftDriveMotor, &rightDriveMotor), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
jogTimer(),
shooterTimer()
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
m_jogTimerRunning = false;
m_shiftCount = MAX_SHIFTS;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
leftDriveEncoder.SetMaxPeriod(1.0);
leftDriveEncoder.SetReverseDirection(true); // change to true if necessary
leftDriveEncoder.Start();
}
/**
* Runs during test mode ```````
*/
void Test() {
TestMode tester(m_ds);
driveDistanceRight.Reset();
driveDistanceRight.Start();
ballGrabber.resetSetPoint();
shooter.motorShutOff();
while (IsTest() && IsEnabled()){
lcd->Clear();
tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick,
&testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic,
&ballGrabber.ballDetector, &analogTestSwitch,
&shooter, &ballGrabber
);
lcd->UpdateLCD();
Wait(0.1);
}
driveDistanceRight.Stop();
}
/**
* 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()
{
control->initialize();
while (IsOperatorControl())
{
control->run();
dsLCD->UpdateLCD();
Wait(0.005);
}
}
void DataSending::UpdateUserLCD(){
char line1[200];
char line2[200];
char line3[200];
char line4[200];
string setting = "Shifter is "+GetGearSetting();
strcpy(line1,setting.c_str());
if((bool)RobotMap::launcherCompressor->GetPressureSwitchValue())sprintf(line2,"Sufficent Pressure");
else sprintf(line2,"Insufficient Pressure");
if(((batteryCurrent->GetVoltage()-2.5)*AMPS_CONSTANT)<150)sprintf(line3,"Don't drop the clutch!");
else sprintf(line3,"You droped the clutch!");
sprintf(line4, "Battery Current is %f Amps",((batteryCurrent->GetVoltage()-2.5)*AMPS_CONSTANT));
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "%s",line1);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "%s",line2);
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "%s",line3);
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%s",line4);
lcd->UpdateLCD();
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous()
{
control->initializeAuto();
while (IsAutonomous())
{
control->runAuto();
dsLCD->UpdateLCD();
Wait(0.005);
}
}
FRC2994_2014():
leftFrontDrive(LEFT_FRONT_DRIVE_PWM),
leftRearDrive(LEFT_REAR_DRIVE_PWM),
rightFrontDrive(RIGHT_FRONT_DRIVE_PWM),
rightRearDrive(RIGHT_REAR_DRIVE_PWM),
leftCenterDrive(CENTER_LEFT_DRIVE_PWM),
rightCenterDrive(CENTER_RIGHT_DRIVE_PWM),
intake(INTAKE_MOTOR_PWM),
rightWinch(RIGHT_WINCH_MOTOR_PWM),
leftWinch(LEFT_WINCH_MOTOR_PWM),
robotDrive(leftFrontDrive, leftRearDrive, leftCenterDrive, rightCenterDrive, rightFrontDrive, rightRearDrive),
rightStick(RIGHT_DRIVE_STICK),
leftStick(LEFT_DRIVE_STICK),
gamepad(GAMEPAD_PORT),
shifters(SHIFTER_A, SHIFTER_B),
arm(ARM_A, ARM_B),
eject(EJECT_A, EJECT_B),
winchSwitch(WINCH_SWITCH),
leftDriveEncoder(LEFT_ENCODER_A, LEFT_ENCODER_B),
rightDriveEncoder(RIGHT_ENCODER_A, RIGHT_ENCODER_B),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
// Robot starts off in a loaded state so a ball can be fit in
loaded(true),
loading(false),
armDown(false)
{
// Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE.
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, __DATE__ " " __TIME__);
dsLCD->UpdateLCD();
ds = DriverStation::GetInstance();
// Set the experation to 1.5 times the loop speed.
robotDrive.SetExpiration(LOOP_PERIOD*1.5);
leftDriveEncoder.SetReverseDirection(true);
}
void OperatorControl(void)
{
// Loop counter to ensure that the program us running (debug helper
// that can be removed when things get more stable)
int sanity, bigSanity = 0;
while (IsOperatorControl() && IsEnabled())
{
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
if (0 == sanity % 20)
{
bigSanity++;
}
Wait(1.0); // wait for a motor update time
}
}
virtual void RobotInit() {
CommandBase::init();
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->UpdateLCD();
//Wait(0.1);
//_autonomousCommand = new AutonomousCommand();
_autonomousCommand = NULL;
_teleopCommand = NULL;
_moveToAndDropBridgeCmd = new MoveToAndDropBridgeCmd();
_noOpCmd = new NoOpCmd();
_driveInSquareCmd = new DriveInSquareCmd();
_kinectDriveCmd = new KinectDriveCmd(CommandBase::oi->kinectLeft, CommandBase::oi->kinectRight, CommandBase::oi->kinectBridgeBtn);
_testRobotCmd = new TestRobotCmd(CommandBase::oi->driveTrigger);
_autoFireCmd = new AutonomousFireCmd();
//_driveWithJoysticksCmd = new DriveWithJoysticksCmd();
_autoChooser = new SendableChooser();
// _autoChooser->AddDefault("Autonomous Fire", _autoFireCmd);
_autoChooser->AddDefault("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
// _autoChooser->AddObject("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
_autoChooser->AddObject("Autonomous Fire", _autoFireCmd);
_autoChooser->AddObject("Drive with Kinect", _kinectDriveCmd);
_autoChooser->AddObject("Drive In Square", _driveInSquareCmd);
SmartDashboard::GetInstance()->PutData("AutoMenu", _autoChooser);
_teleopChooser = new SendableChooser();
_teleopChooser->AddDefault("Drive With Joysticks", _noOpCmd);
_teleopChooser->AddObject("Test Robot", _testRobotCmd);
SmartDashboard::GetInstance()->PutData("TeleopMenu", _teleopChooser);
// SmartDashboard::GetInstance()->PutData("SchedulerData", Scheduler::GetInstance());
// SmartDashboard::GetInstance()->PutData("DriveTrainSubsystem", CommandBase::driveTrainSubsystem);
// SmartDashboard::GetInstance()->PutData("AzimuthSubsystem", CommandBase::azimuthSubsystem);
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
Saftey->SetEnabled(false);
//myRobot->SetSafetyEnabled(false);
//myRobot->Drive(0.5, 0.0); // drive forwards half speed
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" );
//dsLCD->UpdateLCD();
Wait(0.5);
ds = DriverStation::GetInstance();
switch(ds->GetLocation())
{
case 1:
//Execute Autonomous code #1
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1");
break;
case 2:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2");
//Execute Autonomous code #2
break;
case 3:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3");
//Execute Autonomous code #3
break;
}
dsLCD->UpdateLCD();
Saftey->SetEnabled(false);
Wait(0.5); // for 2 seconds
delete Jaguar1;
delete Jaguar2;
delete Saftey;
delete dsLCD;
delete ds;
}