void RunLaps::executePlay(VisionData* vision, RobocupStrategyData* rsd)
{
for (RobotIndex robot = ROBOT0; robot < NUM_ROBOTS; robot++)
{
if (rsd->getPositionOfRobot(robot) != NO_POSITION) {
Destination* command = rsd->getDestination(robot);
command->setSpeed(DEFAULT_SPEED);
command->setControl(OMNI_NORMAL_ENTERBOX);
const SystemParameters& p = rsd->getSystemParams();
switch(lapState[robot])
{
case runningUp:
rsd->setMessage(robot, "Running Up the field");
command->setPos(p.field.THEIR_GOAL_LINE - DIST_FROM_TARGET_WALL, p.field.LEFT_SIDE_LINE - DIST_FROM_OTHER_WALL);
command->setRotation(0);
if(getXLocation(robot, *vision, p) > p.field.THEIR_GOAL_LINE - THRESH_DIST_FROM_WALL)
{
lapState[robot] = runningRight;
}
break;
case runningRight:
rsd->setMessage(robot, "Running right");
command->setPos(p.field.THEIR_GOAL_LINE - DIST_FROM_OTHER_WALL, p.field.RIGHT_SIDE_LINE + DIST_FROM_TARGET_WALL);
command->setRotation(-PI/2);
if(getYLocation(robot, *vision, p) < p.field.RIGHT_SIDE_LINE + THRESH_DIST_FROM_WALL)
{
lapState[robot] = runningDown;
}
break;
case runningDown:
rsd->setMessage(robot, "Running down the field");
command->setPos(p.field.OUR_GOAL_LINE + DIST_FROM_TARGET_WALL, p.field.RIGHT_SIDE_LINE + DIST_FROM_OTHER_WALL);
command->setRotation(PI);
if(getXLocation(robot, *vision, p) < p.field.OUR_GOAL_LINE + THRESH_DIST_FROM_WALL)
{
lapState[robot] = runningLeft;
}
break;
case runningLeft:
rsd->setMessage(robot, "Running left");
command->setPos(p.field.OUR_GOAL_LINE + DIST_FROM_OTHER_WALL, p.field.LEFT_SIDE_LINE - DIST_FROM_TARGET_WALL);
command->setRotation(PI/2);
if(getYLocation(robot, *vision, p) > p.field.LEFT_SIDE_LINE - THRESH_DIST_FROM_WALL)
{
lapState[robot] = runningUp;
}
break;
default:
rsd->setMessage(robot, "internal error in runLaps");
break;
}
command->setRotation(0);
}
}
}
void PredictionTestPlay::executePlay(VisionData* vision, RobocupStrategyData* rsd)
{
for (RobotIndex robot = ROBOT0; robot < NUM_ROBOTS; robot++)
{
if (rsd->getPositionOfRobot(robot) != NO_POSITION) {
Destination* command = rsd->getDestination(robot);
command->setSpeed(DEFAULT_SPEED);
command->setControl(OMNI_NORMAL_ENTERBOX);
const SystemParameters& p = rsd->getSystemParams();
switch(testLapState[robot])
{
case runningUpLong:
if( testWaitState[robot] == waiting )
{
if( waitCounter < MAX_WAIT )
{
waitCounter++;
rsd->setMessage(robot, "Waiting...");
command->setRotation(rot);
}
else
{
testLapState[robot] = runningRightCross;
testWaitState[robot] = running;
waitCounter = 0;
}
}
else
{
rsd->setMessage(robot, "Running Up the field");
command->setPos(p.field.THEIR_GOAL_LINE - DIST_FROM_TARGET_WALL, p.field.LEFT_SIDE_LINE - DIST_FROM_OTHER_WALL);
command->setRotation(rot);
if(getXLocation(robot, *vision, p) > p.field.THEIR_GOAL_LINE - THRESH_DIST_FROM_WALL)
testWaitState[robot] = waiting;
}
break;
case runningRightCross:
if( testWaitState[robot] == waiting )
{
if( waitCounter < MAX_WAIT )
{
waitCounter++;
rsd->setMessage(robot, "Waiting...");
command->setRotation(rot);
}
else
{
testLapState[robot] = runningDownLong;
testWaitState[robot] = running;
waitCounter = 0;
}
}
else
{
rsd->setMessage(robot, "Running right");
command->setPos(p.field.THEIR_GOAL_LINE - DIST_FROM_OTHER_WALL, p.field.RIGHT_SIDE_LINE + DIST_FROM_TARGET_WALL);
command->setRotation(rot);
if(getYLocation(robot, *vision, p) < p.field.RIGHT_SIDE_LINE + THRESH_DIST_FROM_WALL)
testWaitState[robot] = waiting;
}
break;
case runningDownLong:
if( testWaitState[robot] == waiting )
{
if( waitCounter < MAX_WAIT )
{
waitCounter++;
rsd->setMessage(robot, "Waiting...");
command->setRotation(rot);
}
else
{
testLapState[robot] = runningLeftCross;
testWaitState[robot] = running;
waitCounter = 0;
}
}
else
{
rsd->setMessage(robot, "Running down the field");
command->setPos(p.field.OUR_GOAL_LINE + DIST_FROM_TARGET_WALL, p.field.RIGHT_SIDE_LINE + DIST_FROM_OTHER_WALL);
command->setRotation(rot);
if(getXLocation(robot, *vision, p) < p.field.OUR_GOAL_LINE + THRESH_DIST_FROM_WALL)
testWaitState[robot] = waiting;
}
break;
case runningLeftCross:
if( testWaitState[robot] == waiting )
{
if (waitCounter < MAX_WAIT )
{
waitCounter++;
rsd->setMessage(robot, "Waiting...");
command->setRotation(rot);
}
else
{
testLapState[robot] = runningUpLong;
testWaitState[robot] = running;
waitCounter = 0;
//finished lap, so turn 45 degrees,and do another
rot += PI/4;
normalizeAngle(rot);
}
}
else
{
char output[200];
sprintf(output, "Running left, rot = %.2f", rot);
rsd->setMessage(robot,output);
command->setPos(p.field.OUR_GOAL_LINE + DIST_FROM_OTHER_WALL, p.field.LEFT_SIDE_LINE - DIST_FROM_TARGET_WALL);
command->setRotation(rot);
if(getYLocation(robot, *vision, p) > p.field.LEFT_SIDE_LINE - THRESH_DIST_FROM_WALL)
testWaitState[robot] = waiting;
}
break;
default:
rsd->setMessage(robot, "internal error in prediction_test_play");
break;
}
}
}
}