AutoModeController::AutoModeController() : m_Timer(new Timer()), m_CurrentCommand(RobotCommand()), m_Fired(false), m_SmartLoaded(false), m_ChangedSetpoint(false) { m_Timer->Start(); reset(); }
AutoModes::AutoModes() { // Set up our selection order m_Modes["Can Burglar!"]; m_Modes["Can Burglar!"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, 0, 0, true, 0.545)); m_Modes["Can Burglar!"].push_back(RobotCommand(CMD_HOLD_DISTANCE, 100, 0, 0, PINCH, 0, 0, true, 2)); m_Modes["Can Burglar!"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, 0, 0, false, 1)); m_Modes["Can Burglar!"].push_back(RobotCommand(CMD_HOLD_DISTANCE, 24, 0, 0, PINCH, 0, 0, false, 2)); m_Modes["Nothing"]; m_Modes["Nothing"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, CONSTANT("PINCHER_CAN"), 0, false, 1)); m_Modes["Two Can"]; m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, CONSTANT("PINCHER_CAN"), 0, false, 0.1875)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.1875)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.125)); m_Modes["Two Can"].push_back(RobotCommand(CMD_TURN, 0, 90, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.5)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 70, 90, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 30, 100, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 30, 120, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 60, 150, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 150, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.375)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 150, 0, PINCH, 0, 0, false, 0.375)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -30, 150, 0, PINCH, 0, 0, false, 0.5)); m_Modes["Two Can"].push_back(RobotCommand(CMD_TURN, 0, 0, 0, PINCH, 0, 0, false, 0.5)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 36, 0, 0, PINCH, 0, -1, false, 1.5)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, CONSTANT("PINCHER_CAN"), -1, false, 0.625)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), -1, false, 0.625)); m_Modes["Two Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -90, 0, CONSTANT("VERTICAL_BASE_TOTE"), PINCH, CONSTANT("PINCHER_CAN"), -1, false, 1.5)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.375)); m_Modes["Two Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, 0, 0, false, 0.375)); m_Modes["Three Can"]; m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, CONSTANT("PINCHER_CAN"), 0, false, 0.1875)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.1875)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.125)); m_Modes["Three Can"].push_back(RobotCommand(CMD_TURN, 0, 90, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.5)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 70, 90, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 30, 100, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 30, 120, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 60, 150, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.45)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 150, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 150, 0, PINCH, 0, 0, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -40, 150, 0, PINCH, 0, 0, false, 0.5)); m_Modes["Three Can"].push_back(RobotCommand(CMD_TURN, 0, 0, 0, PINCH, 0, 0, false, 0.5)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 45, 0, 0, PINCH, CONSTANT("PINCHER_CAN"), -1, false, 0.625)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), -1, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), -0.5, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_TURN, 0, -180, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), -0.5, false, 1)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 65, -180, CONSTANT("VERTICAL_BASE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), 0, false, 1)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, -180, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, -180, 0, PINCH, 0, 0, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -72, -180, 0, PINCH, 0, 0, false, 1)); m_Modes["Three Can"].push_back(RobotCommand(CMD_TURN, 0, -90, 0, PINCH, 0, 0, false, 1)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 130, -90, 0, PINCH, CONSTANT("PINCHER_CAN"), -1, false, 1.5)); m_Modes["Three Can"].push_back(RobotCommand(CMD_TURN, 0, -180, CONSTANT("VERTICAL_BASE_TOTE") - 3, GRAB, CONSTANT("PINCHER_CAN"), -0.5, false, 1)); m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 60, -180, CONSTANT("VERTICAL_BASE_TOTE") - 3, GRAB, CONSTANT("PINCHER_CAN"), -0.5, false, 1)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), 0, false, 0.375)); m_Modes["Three Can"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, 0, 0, false, 0.375)); // m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 45, 230, 0, PINCH, 0, 0, 0.75)); // m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 60, 270, 0, PINCH, 0, 0, 0.75)); // m_Modes["Three Can"].push_back(RobotCommand(CMD_TURN, 0, 270, 0, PINCH, 0, -0.5, 1)); // m_Modes["Three Can"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 110, 270, 0, PINCH, 0, 0, 2)); m_Modes["Two Tote"]; m_Modes["Two Tote"].push_back(RobotCommand(CMD_TURN, 0, -45, 0, PINCH, 0, 0, false, 0.375)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 12, -45, 0, PINCH, CONSTANT("PINCHER_CAN"), -1, false, 0.75)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -12, -45, 0, GRAB, CONSTANT("PINCHER_CAN"), -1, false, 0.75)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_TURN, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), -1, false, 0.375)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 130, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), -0.2, false, 1.875)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -45, 0, 0, PINCH, 0, 0, false, 1)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_TURN, 0, -45, 0, PINCH, 0, 0, false, 0.375)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 22, -45, 0, PINCH, CONSTANT("PINCHER_CAN"), -1, false, 0.75)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -18, -45, 0, GRAB, CONSTANT("PINCHER_CAN"), -1, false, 0.75)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_TURN, 0, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), -0.2, false, 0.75)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 48.5, 0, CONSTANT("VERTICAL_ONE_TOTE"), GRAB, CONSTANT("PINCHER_CAN"), -0.2, false, 0.75)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_WAIT, 48.5, 0, CONSTANT("VERTICAL_ONE_TOTE"), PINCH, 0, 0, false, 0.6)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -5, 0, 0, PINCH, 0, 0, false, 0.5)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 10, 0, 0, PINCH, 0, 0, false, 0.5)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, PINCH, CONSTANT("PINCHER_CAN"), -1, false, 0.1875)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 80, 0, 0, GRAB, CONSTANT("PINCHER_CAN"), -0.2, false, 0.50)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_TURN_WITH_TOTE, 0, 90, 0, GRAB, CONSTANT("PINCHER_CAN"), -0.2, false, 1)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 90, 90, 0, GRAB, CONSTANT("PINCHER_CAN"), -0.2, false, 0.6)); m_Modes["Two Tote"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -70, 90, 0, PINCH, 0, 0, false, 0.6)); // m_Modes["Default"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -170, 90, 0, PINCH, 0, 0, 1)); // m_Modes["Default"].push_back(RobotCommand(CMD_TURN, 0, 0, 0, PINCH, 0, 0, 0.75)); // m_Modes["Default"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -10, 0, 0, PINCH, 0, 0, 0.25)); // m_Modes["Default"].push_back(RobotCommand(CMD_TURN, 0, -45, 0, PINCH, 0, 0, 0.75)); // m_Modes["Default"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, 0.75)); //dont run this for now // m_Modes["Default"].push_back(RobotCommand(CMD_TURN, 0, -45, 0, 0.75)); // m_Modes["Default"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 12, -45, 0, 0.75)); // m_Modes["Default"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, -12, -45, 0, 0.75)); // m_Modes["Default"].push_back(RobotCommand(CMD_TURN, 0, 90, CONSTANT("VERTICAL_TWO_TOTE"), 0.75)); // m_Modes["Default"].push_back(RobotCommand(CMD_DRIVE_DISTANCE, 90, 90, CONSTANT("VERTICAL_TWO_TOTE"), 1.5)); // // m_Modes["Default"].push_back(RobotCommand(CMD_WAIT, 0, 0, 0, 5)); m_Iterator = m_Modes.begin(); }
void AutoModeController::handle(CowRobot* bot) { bool result = false; // Run the command switch(m_CurrentCommand.m_Command) { case CMD_NULL: { doNothing(bot); result = true; break; } case CMD_WAIT: { bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); doNothing(bot); break; } case CMD_DESPRING: { doNothing(bot); bot->WinchDespring(); break; } case CMD_DETECT_HOT: { doNothing(bot); if(bot->KinectLeftRight() < -0.5) { m_CommandList = *(m_CurrentCommand.m_HotGoalLeftCommandList); printf("Detected kinect left!\n"); result = true; } else if(bot->KinectLeftRight() > 0.5) { m_CommandList = *(m_CurrentCommand.m_HotGoalRightCommandList); printf("Detected kinect right!\n"); result = true; } break; } case CMD_CHANGE_SETPOINT: { bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); if(!m_ChangedSetpoint) { bot->ChangeWinchSetpoint(m_CurrentCommand.m_WinchSetpoint); m_ChangedSetpoint = true; } else { if(bot->ReadyToFire()) { m_ChangedSetpoint = false; result = true; } } break; } case CMD_FORCE_FIRE: { bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(!m_Fired) { bot->ForceFire(); m_Fired = true; } else if(bot->ReadyToFire()) { result = true; m_Fired = false; } break; } case CMD_FIRE: { bot->DriveDistanceWithHeading(m_CurrentCommand.m_Heading, m_CurrentCommand.m_EncoderCount); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(!m_Fired) { if(bot->ReadyToFire()) { bot->AskForFire(); m_Fired = true; } } else { // if we have fired if(!m_ChangedSetpoint) { m_ChangedSetpoint = bot->ChangeWinchSetpoint(m_CurrentCommand.m_WinchSetpoint); } if(bot->Reloading()) { result = true; m_Fired = false; } } break; } case CMD_FIRE_HEADING_ONLY: { bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); //bot->DriveDistanceWithHeading(m_CurrentCommand.m_Heading, m_CurrentCommand.m_EncoderCount); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(!m_Fired) { if(bot->ReadyToFire()) { bot->AskForFire(); m_Fired = true; } } else { // if we have fired if(!m_ChangedSetpoint) { m_ChangedSetpoint = bot->ChangeWinchSetpoint(m_CurrentCommand.m_WinchSetpoint); } if(bot->Reloading()) { result = true; m_Fired = false; } } break; } case CMD_FIRE_INSTANT: { bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(bot->ReadyToFire()) { bot->AskForFire(); result = true; } break; } case CMD_FIRE_INTAKE: { bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(bot->ReadyToFire()) { bot->FrontIntake(0, m_CurrentCommand.m_FrontIntakeState); if(!m_Fired) { bot->AskForFire(); m_Fired = true; } else { result = true; m_Fired = false; } } else if(m_Fired) { bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); } else { bot->FrontIntake(0, m_CurrentCommand.m_FrontIntakeState); } break; } case CMD_DRIVE_FIRE_SKIP: { bot->DriveDistanceWithHeading(m_CurrentCommand.m_Heading, m_CurrentCommand.m_EncoderCount); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(bot->GetDriveDistance() > m_CurrentCommand.m_FirePoint) { if(!m_Fired) { if(bot->ReadyToFire()) { bot->AskForFire(); m_Fired = true; } } else { // if we have fired if(!m_ChangedSetpoint) { m_ChangedSetpoint = bot->ChangeWinchSetpoint(m_CurrentCommand.m_WinchSetpoint); } if(bot->Reloading()) { result = true; m_Fired = false; } } } break; } case CMD_DRIVE_FIRE: { bool drive = bot->DriveDistanceWithHeading(m_CurrentCommand.m_Heading, m_CurrentCommand.m_EncoderCount); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); if(bot->ReadyToFire() && bot->GetDriveDistance() > m_CurrentCommand.m_FirePoint) { if(!m_Fired) { bot->AskForFire(); m_Fired = true; } else { // if we have fired if(!m_ChangedSetpoint) { m_ChangedSetpoint = bot->ChangeWinchSetpoint(m_CurrentCommand.m_WinchSetpoint); } if(drive) { result = true; m_Fired = false; } } } break; } case CMD_INTAKES: { bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); break; } case CMD_INTAKES_FRONT: { //Figure out if we actually sucked in the ball if(bot->GetRearIR() > 1) { result = true; } bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); break; } case CMD_TURN: { bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); result = bot->DriveWithHeading(m_CurrentCommand.m_Heading, 0); break; } case CMD_DRIVE_DISTANCE: { bot->FrontIntake(m_CurrentCommand.m_FrontIntakeSpeed, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(m_CurrentCommand.m_RearIntakeSpeed, m_CurrentCommand.m_RearIntakeState); result = bot->DriveDistanceWithHeading(m_CurrentCommand.m_Heading, m_CurrentCommand.m_EncoderCount); break; } case CMD_SETTLE_BALL: { bot->FrontIntake(0, m_CurrentCommand.m_FrontIntakeState); bot->RearIntake(0, m_CurrentCommand.m_RearIntakeState); // bot->SetFrontIntakeState(m_CurrentCommand.m_FrontIntakeState); // bot->SetRearIntakeState(m_CurrentCommand.m_RearIntakeState); bot->AutoSettle(); result = (bot->GetSettledState() == CowRobot::SETTLE_FINISHED); break; } default: { doNothing(bot); result = true; break; } } // Check if this command is done if(result == true || m_CurrentCommand.m_Command == CMD_NULL || m_Timer->Get() > m_CurrentCommand.m_Timeout) { if(m_CurrentCommand.m_Command == CMD_DETECT_HOT && m_Timer->Get() > m_CurrentCommand.m_Timeout) { m_CommandList = *(m_CurrentCommand.m_HotGoalLeftCommandList); } // This command is done, go get the next one if(m_CommandList.size() > 0 ) { m_CurrentCommand = m_CommandList.front(); m_CommandList.pop_front(); if(!m_CurrentCommand.m_Command == CMD_NULL) printf("Time elapsed: %f\n", m_Timer->Get()); m_Timer->Reset(); m_Fired = false; m_SmartLoaded = false; m_ChangedSetpoint = false; } else { //we're done clean up m_CurrentCommand = RobotCommand(); } } }