// 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; }