// Test the DriveDistance and RotateDegrees messages
IEnumerator <ITask> TestDriveDistanceAndRotateDegrees()
{
yield return(Arbiter.Choice(
_mainPort.RotateDegrees(90, 0.2),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f) { LogInfo(LogGroups.Console, "RotateDegrees Fault"); }
));
yield return(Arbiter.Choice(
_mainPort.RotateDegrees(-90, 0.2),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f) { LogInfo(LogGroups.Console, "RotateDegrees Fault"); }
));
}
/// <summary>
/// Turns the drive relative to its current heading.
/// </summary>
/// <param name="angle">angle in degrees</param>
/// <returns>response port</returns>
PortSet <DefaultUpdateResponseType, Fault> Turn(int angle)
{
if (_state.DriveState == null || !_state.DriveState.IsEnabled)
{
EnableMotor();
}
drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
request.Degrees = (double)(-angle);
return(_drivePort.RotateDegrees(request));
}
// Iterator to execute the Behavior
// It is important to use an Iterator so that it can relinquish control
// when there is nothing to do, i.e. yield return
IEnumerator <ITask> Behavior()
{
// Wait for the robot to initialize, otherwise it will
// miss the initial command
for (int i = 10; i > 0; i--)
{
LogInfo(LogGroups.Console, i.ToString());
yield return(Timeout(1000));
}
if (useTimers)
{
LogInfo(LogGroups.Console, "Starting now using Timers ...");
}
else
{
LogInfo(LogGroups.Console, "Starting now using Controlled Moves ...");
}
// Make sure that the drive is enabled first!
_drivePort.EnableDrive(true);
for (int times = 0; times < repeatCount; times++)
{
// Drive along the four sides of a square
for (int side = 0; side < 4; side++)
{
// Display progress info for the user
LogInfo(LogGroups.Console, "Side " + side);
if (useTimers)
{
// This appoach just uses SetDrivePower and sets timers
// to control the drive and rotate motions. This is not
// very accurate and the exact timer parameters will be
// different for different types of robots.
// Start driving and then wait for a while
_drivePort.SetDrivePower(drivePower, drivePower);
yield return(Timeout(driveTime));
// Stop the motors and wait for robot to settle
_drivePort.SetDrivePower(0, 0);
yield return(Timeout(settlingTime));
// Now turn left and wait for a different amount of time
_drivePort.SetDrivePower(-rotatePower, rotatePower);
yield return(Timeout(rotateTime));
// Stop the motors and wait for robot to settle
_drivePort.SetDrivePower(0, 0);
yield return(Timeout(settlingTime));
}
else
{
// This code uses the DriveDistance and RotateDegrees
// operations to control the robot. These are not precise,
// but they should be better than using timers and they
// should also work regardless of the type of robot.
bool success = true;
Fault fault = null;
// Drive straight ahead
yield return(Arbiter.Choice(
_drivePort.DriveDistance(driveDistance, drivePower),
delegate(DefaultUpdateResponseType response) { success = true; },
delegate(Fault f) { success = false; fault = f; }
));
// If the DriveDistance was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
// NOTE: This approach only works if you always wait for a
// completion message. If you send any other drive request
// while the current one is active, then the current motion
// will be canceled, i.e. cut short.
if (success)
{
yield return(WaitForCompletion());
}
else
{
LogError("Error occurred on DriveDistance: " + fault);
}
// Wait for settling time
yield return(Timeout(settlingTime));
// Now turn left
yield return(Arbiter.Choice(
_drivePort.RotateDegrees(rotateAngle, rotatePower),
delegate(DefaultUpdateResponseType response) { success = true; },
delegate(Fault f) { success = false; fault = f; }
));
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (success)
{
yield return(WaitForCompletion());
}
else
{
LogError("Error occurred on RotateDegrees: " + fault);
}
// Wait for settling time
yield return(Timeout(settlingTime));
}
}
}
// And finally make sure that the robot is stopped!
_drivePort.SetDrivePower(0, 0);
LogInfo(LogGroups.Console, "Finished");
yield break;
}