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();
}
}
}
