State_t Striker::DoStateGameControllerReady()
{
XYLocation_s positionLocation;
if (MyRobot::GetPlayerID() == ID_STRIKER1)
{
if (m_communication->GetkickOffTeam() == m_communication->teamId)
{
positionLocation.x = OFFENSIVE_POSITION_STRIKER1_X;
positionLocation.y = OFFENSIVE_POSITION_STRIKER1_Y;
}
else
{
positionLocation.x = DEFENSIVE_POSITION_STRIKER1_X;
positionLocation.y = DEFENSIVE_POSITION_STRIKER1_Y;
}
}
else if (MyRobot::GetPlayerID() == ID_STRIKER2)
{
if (m_communication->GetkickOffTeam() == m_communication->teamId)
{
positionLocation.x = OFFENSIVE_POSITION_STRIKER2_X;
positionLocation.y = OFFENSIVE_POSITION_STRIKER2_Y;
}
else
{
positionLocation.x = DEFENSIVE_POSITION_STRIKER2_X;
positionLocation.y = DEFENSIVE_POSITION_STRIKER2_Y;
}
}
else
{
throw "UNKNOWN_ROBOT_ID";
}
m_vision->SetHeadTrackingStatus(GOAL_TRACKING);
m_goTo->Stop();
while(exitProgram == false)
{
m_goTo->QuickAndDirty(positionLocation.x,positionLocation.y ,0);
if (!IsStateReady())
{
m_vision->SetHeadTrackingStatus(BALL_TRACKING);
m_goTo->Stop();
return STATE_HANDLE_GAME_CONTROLLER_STATES;
}
usleep(100 *1000);
}
return STATE_HANDLE_GAME_CONTROLLER_STATES; // TODO Fix this. need to be in a while.
// TODO add ready points with an id for each striker, instead of these calculations
}
bool
SmartTaskAdvance::CheckReadyToAdvance(const TaskPoint &tp,
const AircraftState &aircraft,
const bool x_enter, const bool x_exit)
{
const bool state_ready = IsStateReady(tp, aircraft, x_enter, x_exit);
if (armed)
request_armed = false;
switch (tp.GetType()) {
case TaskPointType::UNORDERED:
gcc_unreachable();
case TaskPointType::START: {
const StartPoint &sp = (const StartPoint &)tp;
if (sp.DoesRequireArm()) {
if (armed) {
state = TaskAdvance::START_ARMED;
} else {
state = TaskAdvance::START_DISARMED;
if (sp.IsInSector(aircraft))
request_armed = true;
}
return armed && state_ready;
} else {
state = TaskAdvance::AUTO;
return state_ready;
}
}
case TaskPointType::AAT:
if (armed) {
state = TaskAdvance::TURN_ARMED;
} else {
state = TaskAdvance::TURN_DISARMED;
if (state_ready)
request_armed = true;
}
return armed && state_ready;
case TaskPointType::AST: {
state = TaskAdvance::AUTO;
return state_ready;
}
case TaskPointType::FINISH:
return false;
}
gcc_unreachable();
}
State_t Goalkeeper::DoStateGameControllerReady() {
XYLocation_s positionLocation;
positionLocation.x = 0;
positionLocation.y = GOALKEEPER_POSITION_Y; //a little ahead of the goal line
m_vision->SetHeadTrackingStatus(GOAL_TRACKING);
while (exitProgram == false) {
m_goTo->QuickAndDirty(positionLocation.x, positionLocation.y, 0);
if (!IsStateReady()) {
m_vision->SetHeadTrackingStatus(BALL_TRACKING);
m_goTo->Stop();
return STATE_HANDLE_GAME_CONTROLLER_STATES;
}
usleep(100 * 1000);
}
return STATE_HANDLE_GAME_CONTROLLER_STATES;
}