/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that everything happens
* in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
* or Test when the robot is enabled. After running the correct method, wait for some state to
* change, either the other mode starts or the robot is disabled. Then go back and wait for the
* robot to be enabled again.
*/
void SampleRobot::StartCompetition()
{
LiveWindow *lw = LiveWindow::GetInstance();
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
RobotMain();
if (!m_robotMainOverridden)
{
// first and one-time initialization
lw->SetEnabled(false);
RobotInit();
while (true)
{
if (IsDisabled())
{
m_ds.InDisabled(true);
Disabled();
m_ds.InDisabled(false);
while (IsDisabled()) sleep(1); //m_ds.WaitForData();
}
else if (IsAutonomous())
{
m_ds.InAutonomous(true);
Autonomous();
m_ds.InAutonomous(false);
while (IsAutonomous() && IsEnabled()) sleep(1); //m_ds.WaitForData();
}
else if (IsTest())
{
lw->SetEnabled(true);
m_ds.InTest(true);
Test();
m_ds.InTest(false);
while (IsTest() && IsEnabled()) sleep(1); //m_ds.WaitForData();
lw->SetEnabled(false);
}
else
{
m_ds.InOperatorControl(true);
OperatorControl();
m_ds.InOperatorControl(false);
while (IsOperatorControl() && IsEnabled()) sleep(1); //m_ds.WaitForData();
}
}
}
}
void Test()
{
while (IsTest() && IsEnabled())
{
SmartDashboard::PutString("DB/String 0", "Test");
}
}
// Runs during test mode
// Test
// *
void Test()
{
shifters.Set(DoubleSolenoid::kForward);
leftDriveEncoder.Start();
leftDriveEncoder.Reset();
int start = leftDriveEncoder.Get();
while (IsTest()) {
if (rightStick.GetRawButton(7)) {
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
else {
robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
}
if (gamepad.GetEvent(4) == kEventClosed) {
start = leftDriveEncoder.Get();
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
dsLCD->UpdateLCD();
gamepad.Update();
}
}
void Test()
{
while(IsTest())
{
//do stuff
}
}
void Test()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while (IsTest())
{
if ((buttonOne.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
}
if ((buttonTwo.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
}
if ((togglebuttonOne.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_Off!");
}
if ((togglebuttonTwo.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_Off!");
}
if (buttonOne.Get() == 0)
{
emergencyCompressor.Set(Relay::kOff);
}
#if 0
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"ButtonOneIs_%d", buttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"toggle_%d", buttonTwo.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Button_%d", togglebuttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"toggle_%d", togglebuttonTwo.Get());
#endif
Wait(0.005);
screen -> UpdateLCD();
}
}
void Test(void)
{
compressor->Start();
myRobot.SetSafetyEnabled(true);
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
while(IsTest())
{
GetWatchdog().Feed();
Wait(0.1);
}
}
/**
* 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();
}
void Test() {
robotDrive.SetSafetyEnabled(false);
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 7; //send 0 to arduino
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
bool isSettingUp = true;
pickup.setGrabber(-1);
pickup.setLifter(1);
while (isSettingUp) {
isSettingUp = false;
if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0);
}
else {
isSettingUp = true;
}
if (liftLowerLimit.Get()) {
pickup.setLifter(0);
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsTest() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
/**
* Provide an alternate "main loop" via StartCompetition().
*
* This specific StartCompetition() implements "main loop" behavior like that of the FRC
* control system in 2008 and earlier, with a primary (slow) loop that is
* called periodically, and a "fast loop" (a.k.a. "spin loop") that is
* called as fast as possible with no delay between calls.
*/
void IterativeRobot::StartCompetition()
{
LiveWindow *lw = LiveWindow::GetInstance();
// first and one-time initialization
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
RobotInit();
// loop forever, calling the appropriate mode-dependent function
lw->SetEnabled(false);
while (true)
{
// Call the appropriate function depending upon the current robot mode
if (IsDisabled())
{
// call DisabledInit() if we are now just entering disabled mode from
// either a different mode or from power-on
if(!m_disabledInitialized)
{
lw->SetEnabled(false);
DisabledInit();
m_disabledInitialized = true;
// reset the initialization flags for the other modes
m_autonomousInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
if (NextPeriodReady())
{
// TODO: HALNetworkCommunicationObserveUserProgramDisabled();
DisabledPeriodic();
}
}
else if (IsAutonomous())
{
// call AutonomousInit() if we are now just entering autonomous mode from
// either a different mode or from power-on
if(!m_autonomousInitialized)
{
lw->SetEnabled(false);
AutonomousInit();
m_autonomousInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
if (NextPeriodReady())
{
// TODO: HALNetworkCommunicationObserveUserProgramAutonomous();
AutonomousPeriodic();
}
}
else if (IsTest())
{
// call TestInit() if we are now just entering test mode from
// either a different mode or from power-on
if(!m_testInitialized)
{
lw->SetEnabled(true);
TestInit();
m_testInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady())
{
// TODO: HALNetworkCommunicationObserveUserProgramTest();
TestPeriodic();
}
}
else
{
// call TeleopInit() if we are now just entering teleop mode from
// either a different mode or from power-on
if(!m_teleopInitialized)
{
lw->SetEnabled(false);
TeleopInit();
m_teleopInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_testInitialized = false;
Scheduler::GetInstance()->SetEnabled(true);
}
if (NextPeriodReady())
{
// TODO: HALNetworkCommunicationObserveUserProgramTeleop();
TeleopPeriodic();
}
}
// wait for driver station data so the loop doesn't hog the CPU
m_ds.WaitForData();
}
}
void Test() // DONT TOUCH THIS AREA. I KEEL YOU.
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int counter = 0;
bool solenoidTest=0;
while (IsTest())
{
if(logitech.GetRawButton(9)) //press rightBack
{
solenoidTest=1;
compressor.Start();
}
if(logitech.GetRawButton(10)) //press Start
{
solenoidTest=0;
compressor.Stop();
}
if(solenoidTest)
{
if(logitech.GetRawButton(1)) //press X
{
rightArmSolenoid.Set(DoubleSolenoid::kForward);
leftArmSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(2)) //press A
{
rightArmSolenoid.Set(DoubleSolenoid::kReverse);
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
leftArmSolenoid.Set(DoubleSolenoid::kOff);
rightArmSolenoid.Set(DoubleSolenoid::kOff);
}
if(logitech.GetRawButton(3)) //PRess URTrigger
{
retrievalMotor.Set(logitech.GetRawAxis(2));
}
else
{
retrievalMotor.Set(0);
}
if(logitech.GetRawButton(4))
{
winchMotor.Set(logitech.GetRawAxis(2));
}
else
{
winchMotor.Set(0);
}
if(logitech.GetRawButton(5))
{
ratchetSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(7))
{
ratchetSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
ratchetSolenoid.Set(DoubleSolenoid::kOff);
}
if(logitech.GetRawButton(6))
{
dogSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(8))
{
dogSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
dogSolenoid.Set(DoubleSolenoid::kOff);
}
}
else
{
if(logitech.GetRawButton(1)) //Press X
{
rightFront.Set(logitech.GetRawAxis(2)); //Press left joystick
}
else
{
rightFront.Set(0);
}
if(logitech.GetRawButton(2)) //Press A
{
rightBack.Set(logitech.GetRawAxis(2));
}
else
{
rightBack.Set(0);
}
if(logitech.GetRawButton(3)) //Press B
{
leftFront.Set(logitech.GetRawAxis(2));
}
else
{
leftFront.Set(0);
}
if(logitech.GetRawButton(4)) //Press Y
{
leftBack.Set(logitech.GetRawAxis(2));
}
else
{
leftBack.Set(0);
}
if(logitech.GetRawButton(5)) //Press ULTrigger
{
retrievalMotor.Set(logitech.GetRawAxis(2));
}
else
{
retrievalMotor.Set(0);
}
if(logitech.GetRawButton(6)) //PRess URTrigger
{
winchMotor.Set(logitech.GetRawAxis(2));
}
else
{
winchMotor.Set(0);
}
if(logitech.GetRawButton(7)) //Press LLTrigger
{
ringLight.Set(Relay::kForward);
}
else
{
ringLight.Set(Relay::kOff);
}
if(logitech.GetRawButton(8)) //Press LRTrigger
{
compressor.Start();
}
else
{
compressor.Stop();
}
}
/****** MANUAL LOAD FUNCTION END *****/
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftJoystick: %f", logitech.GetRawAxis(2));
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"RF:%f RB:%f LF:%f LB:%f", rightFront.Get(), rightBack.Get(), leftFront.Get(), leftBack.Get()); // Print WinchMotor State
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Solenoid Testing:%d", solenoidTest);
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"rightArmSolenoid:%d", rightArmSolenoid.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"time:%d", counter);
counter ++;
Wait(0.005); // Waits to run the loop every 0.005 seconds so the cRIO doesn't explode
screen->UpdateLCD();
}
}
/**
* Provide an alternate "main loop" via StartCompetition().
*
* This specific StartCompetition() implements "main loop" behaviour synced with the DS packets
*/
void IterativeRobot::StartCompetition()
{
HALReport(HALUsageReporting::kResourceType_Framework, HALUsageReporting::kFramework_Iterative);
LiveWindow *lw = LiveWindow::GetInstance();
// first and one-time initialization
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
RobotInit();
// We call this now (not in Prestart like default) so that the robot
// won't enable until the initialization has finished. This is useful
// because otherwise it's sometimes possible to enable the robot
// before the code is ready.
HALNetworkCommunicationObserveUserProgramStarting();
// loop forever, calling the appropriate mode-dependent function
lw->SetEnabled(false);
while (true)
{
// Call the appropriate function depending upon the current robot mode
if (IsDisabled())
{
// call DisabledInit() if we are now just entering disabled mode from
// either a different mode or from power-on
if(!m_disabledInitialized)
{
lw->SetEnabled(false);
DisabledInit();
m_disabledInitialized = true;
// reset the initialization flags for the other modes
m_autonomousInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
HALNetworkCommunicationObserveUserProgramDisabled();
DisabledPeriodic();
}
else if (IsAutonomous())
{
// call AutonomousInit() if we are now just entering autonomous mode from
// either a different mode or from power-on
if(!m_autonomousInitialized)
{
lw->SetEnabled(false);
AutonomousInit();
m_autonomousInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
HALNetworkCommunicationObserveUserProgramAutonomous();
AutonomousPeriodic();
}
else if (IsTest())
{
// call TestInit() if we are now just entering test mode from
// either a different mode or from power-on
if(!m_testInitialized)
{
lw->SetEnabled(true);
TestInit();
m_testInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_teleopInitialized = false;
}
HALNetworkCommunicationObserveUserProgramTest();
TestPeriodic();
}
else
{
// call TeleopInit() if we are now just entering teleop mode from
// either a different mode or from power-on
if(!m_teleopInitialized)
{
lw->SetEnabled(false);
TeleopInit();
m_teleopInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_testInitialized = false;
Scheduler::GetInstance()->SetEnabled(true);
}
HALNetworkCommunicationObserveUserProgramTeleop();
TeleopPeriodic();
}
// wait for driver station data so the loop doesn't hog the CPU
m_ds->WaitForData();
}
}
void Test()
{
SmartDashboard::init();
Joystick* driverStick = driverInput->GetDriverJoystick();
//Relay* redLights = new Relay(1, Relay::kForward);
//Relay* blueLights = new Relay(2, Relay::kForward);
//Relay* cameraLights = new Relay(CAMERA_LED);
//Relay* redLights = new Relay(1, Relay::kForward);
//Solenoid *rightSolenoid;
//Solenoid *leftSolenoid;
bool gear = false;
bool prevShiftButttonPressed = false;
Victor* motors [] = {feeder->feederWheel, feeder->feederArm, drive->leftDrive, drive->rightDrive, catapult->ChooChooMotor};
Relay* compressor = new Relay(COMPRESSOR_RELAY);
DigitalInput* compressorLimitSwitch = new DigitalInput(COMPRESSOR_SWITCH);
//Compressor* compressor = new Compressor(1,1);//COMPRESSOR_SWITCH, COMPRESSOR_RELAY);
//compressor->Start();
Relay* cameraLights = new Relay(CAMERA_LED);
//Solenoid* right = new Solenoid(6);
//Solenoid* left = new Solenoid(5);
//right->Set(true);
//left->Set(true);
//Relay* redlights = catapult->redled;
//Relay* whitelights = catapult->whiteled;
//redlights->Set(redlights->kForward);
//whitelights->Set(whitelights->kForward);
//cameraLights->Set(cameraLights->kForward);
bool compressorOn = false;
while (IsTest() && IsEnabled())
{
bool curShiftButtonPressed = driverStick->GetRawButton(10);
//sensors->GetInputs();
//SmartDashboard::PutNumber("Rangefinder", sensors->GetDistance());
/*FEEDER WHEEL*/
if (driverStick->GetRawButton(2))
{
motors[0]->Set(0.4);
}
else
{
motors[0]->Set(0);
}
/*FEEDER ARM*/
if (driverStick->GetRawButton(3))
{
motors[1]->Set(-0.6);
}
else
{
motors[1]->Set(0);
}
/*LEFT DRIVE*/
if (driverStick->GetRawButton(4))
{
motors[2]->Set(0.8);
}
else
{
motors[2]->Set(0);
}
/*RIGHT DRIVE*/
if (driverStick->GetRawButton(5))
{
motors[3]->Set(0.8);
}
else
{
motors[3]->Set(0);
}
/*
if (driverStick->GetRawButton(6))
{
redlights->Set(redlights->kForward);
}
else
{
redlights->Set(redlights->kOff);
}
*/
/*CHOOCHOO MOTOR*/
if (driverStick->GetRawButton(7) && driverStick->GetRawButton(1))
{
motors[4]->Set(0.6);
}
else
{
motors[4]->Set(0);
}
/*CAMERA LED*/
if (driverStick->GetRawButton(8))
{
cameraLights->Set(cameraLights->kForward);
}
else
{
cameraLights->Set(cameraLights->kOff);
}
/*
if (curShiftButtonPressed && !prevShiftButttonPressed)
{
drive->SetHighGear(!gear);
}
*/
if (driverStick->GetRawButton(10))
{
drive->SetHighGear(true);//!gear
gear = !gear;
}
else{
drive->SetHighGear(false);
}
/*COMPRESSOR*/
if(driverStick->GetRawButton(11))
{
compressorOn = true;
//compressor->Set(compressor->kOn);
//compressor->Start();
}
else
{
//compressor->Stop();
}
if(driverStick->GetRawButton(9))
{
compressorOn = false;
//compressor->Set(compressor->kOff);
//compressor->Stop();
}
if(compressorOn){
if(!compressorLimitSwitch->Get()){
compressor->Set(compressor->kOn);
}
else{
compressor->Set(compressor->kOff);
compressorOn = false;
}
}
else
compressor->Set(compressor->kOff);
prevShiftButttonPressed = curShiftButtonPressed;
}
}
void Test()
{
menuType currentMenu = TOP;
menuType newMenu = TOP;
BaseMenu * menus[NUM_MENU_TYPE];
menus[TOP] = new TopMenu;
menus[ANALOG] = new AnalogMenu;
menus[DIGITAL_TOP] = new DigitalMenu;
menus[SOLENOID] = new SolenoidMenu;
menus[DIGITAL_PWM] = new PWMMenu;
menus[DIGITAL_IO] = new DigitalIOMenu;
menus[DIGITAL_RELAY] = new RelayMenu;
menus[DIGITAL_IO_STATE] = new DigitalIOStateMenu;
menus[DIGITAL_IO_CLOCK] = new DigitalIOClockMenu;
menus[DIGITAL_IO_ENCODER] = new DigitalIOEncoderMenu;
// Write out the TOP menu for the first time
menus[currentMenu]->UpdateDisplay();
// Initialize the button states on the gamepad
gamepad.Update();
// Loop counter to ensure that the program us running (debug helper
// that can be removed when things get more stable)
int sanity = 0;
while (IsTest())
{
// The dpad "up" button is used to move the menu pointer up one line
// on the LCD display
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kUp))
{
menus[currentMenu]->HandleIndexUp();
}
// The dpad "down" button is used to move the menu pointer down one line
// on the LCD display
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kDown))
{
menus[currentMenu]->HandleIndexDown();
}
// The dpad left button is used to exit a submenu when the menu pointer
// points to the "back" menu item and to decrease a value (where
// appropriate) on any other menu item.
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kLeft))
{
newMenu = menus[currentMenu]->HandleSelectLeft();
}
// Theoretically, both the select buttons could be pressed in the
// same 10 msec window. However, if using the dpad on the game
// game controller this is physically impossible so we don't
// need to worry about a previous value of newMenu being
// overwritten in the next bit of code.
// The dpad right button is used to enter a submenu when the menu pointer
// points to a submenu item and to increase a value (where appropriate)
// on any other menu item.
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kRight))
{
newMenu = menus[currentMenu]->HandleSelectRight();
// Handle change from one menu to a sub menu
if (newMenu != currentMenu)
{
// When we enter a menu we need to set the record the
// menu to return to. We do *not* want to do this when
// returning from a menu to its calling menu.
menus[newMenu]->SetCallingMenu(currentMenu);
}
}
// Handle change from one menu to another
if (newMenu != currentMenu)
{
menus[newMenu]->UpdateDisplay();
currentMenu = newMenu;
}
// Set the motor speed(s) (if any have been enabled via the Digital PWM menu)
menus[DIGITAL_PWM]->SetSpeed(-1.0 * gamepad.GetRightY());
// Update gamepad button states
gamepad.Update();
// Update the display (we do this on every loop pass because some menus
// (analog, for example) need to have values updated even when there are
// no dpad events to handle)
menus[currentMenu]->UpdateDisplay();
// Dump the sanity time value to the LCD
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Sanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
// Run the loop every 50 msec (20 times per second)
Wait(0.050);
}
}
bool DriverStation::IsOperatorControl()
{
return !(IsAutonomous() || IsTest());
}
void RhsRobotBase::StartCompetition() //Robot's main function
{
DriverStation *pDS = DriverStation::GetInstance();
Init(); //Initialize the robot
while(true)
{
if(!pDS->IsNewControlData())
{
Wait(0.002);
continue;
}
//Checks the current state of the robot
if(IsDisabled())
{
currentRobotState = ROBOT_STATE_DISABLED;
}
else if(IsEnabled() && IsAutonomous())
{
currentRobotState = ROBOT_STATE_AUTONOMOUS;
}
else if(IsEnabled() && IsOperatorControl())
{
currentRobotState = ROBOT_STATE_TELEOPERATED;
}
else if(IsEnabled() && IsTest())
{
currentRobotState = ROBOT_STATE_TEST;
}
else
{
currentRobotState = ROBOT_STATE_UNKNOWN;
}
if(HasStateChanged()) //Checks for state changes
{
switch(GetCurrentRobotState())
{
case ROBOT_STATE_DISABLED:
printf("ROBOT_STATE_DISABLED\n");
robotMessage.command = COMMAND_ROBOT_STATE_DISABLED;
//robotMessage.robotMode = ROBOT_STATE_DISABLED;
break;
case ROBOT_STATE_AUTONOMOUS:
printf("ROBOT_STATE_AUTONOMOUS\n");
robotMessage.command = COMMAND_ROBOT_STATE_AUTONOMOUS;
//robotMessage.robotMode = ROBOT_STATE_AUTONOMOUS;
break;
case ROBOT_STATE_TELEOPERATED:
printf("ROBOT_STATE_TELEOPERATED\n");
robotMessage.command = COMMAND_ROBOT_STATE_TELEOPERATED;
//robotMessage.robotMode = ROBOT_STATE_TELEOPERATED;
break;
case ROBOT_STATE_TEST:
printf("ROBOT_STATE_TEST\n");
robotMessage.command = COMMAND_ROBOT_STATE_TEST;
//robotMessage.robotMode = ROBOT_STATE_TEST;
break;
case ROBOT_STATE_UNKNOWN:
printf("ROBOT_STATE_UNKNOWN\n");
robotMessage.command = COMMAND_ROBOT_STATE_UNKNOWN;
//robotMessage.robotMode = ROBOT_STATE_UNKNOWN;
break;
}
OnStateChange(); //Handles the state change
}
if(IsEnabled())
{
if((currentRobotState == ROBOT_STATE_TELEOPERATED) ||
(currentRobotState == ROBOT_STATE_TEST) ||
(currentRobotState == ROBOT_STATE_AUTONOMOUS))
{
Run(); //Robot logic
}
}
previousRobotState = currentRobotState;
++loop; //Increment the loop counter
}
}