private static DriveAnswerCommand robotDriveAround(ClientRobot robot)
{
if (robot.Power > robot.Motor.ROTATE_IN)
{
return(robot.Drive(robot.AngleDrive, robot.Motor.ROTATE_IN));
}
else
{
if (0 <= robot.AngleDrive && robot.AngleDrive < 90)
{
return(robot.Drive(180, 100));
}
else if (90 <= robot.AngleDrive && robot.AngleDrive < 180)
{
return(robot.Drive(270, 100));
}
else if (180 <= robot.AngleDrive && robot.AngleDrive < 270)
{
return(robot.Drive(0, 100));
}
else if (270 <= robot.AngleDrive && robot.AngleDrive < 360)
{
return(robot.Drive(90, 100));
}
}
return(null);
}
private static DriveAnswerCommand robotDriveToBase(ClientRobot robot)
{
double angle = AngleUtils.AngleDegree(robot.X, robot.Y, capturedBase.X, capturedBase.Y);
if (robot.Power > robot.Motor.ROTATE_IN && Math.Abs(angle - robot.AngleDrive) > 1)
{
return(robot.Drive(robot.AngleDrive, robot.Motor.ROTATE_IN));
}
else
{
return(robot.Drive(angle, 100));
}
}