// 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
protected IEnumerator <ITask> BehaviorTurn()
{
LogInfo("DriveBehaviorServiceBase: BehaviorTurn() Started");
Talker.Say(2, "starting Behavior Turn And Move Forward");
// 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));
}
LogInfo(LogGroups.Console, "Turning " + (rotateAngle > 0.0d ? "Right " : "Left "));
bool success = true;
Fault fault = null;
// Turn:
yield return(Arbiter.Choice(
TurnByAngle((int)rotateAngle, utTurnPower * utPowerScale),
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)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("Error occurred on TurnByAngle: " + fault);
}
// done
yield break;
}
// 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
protected IEnumerator <ITask> BehaviorStraightInterruptTurn_Turn()
{
LogInfo("DriveBehaviorServiceBase: BehaviorStraightInterruptTurn_Turn() Started");
bool success = true;
Fault fault = null;
LogInfo(LogGroups.Console, "Turning " + (rotateAngle > 0.0d ? "Right " : "Left "));
// turn first:
yield return(Arbiter.Choice(
TurnByAngle((int)rotateAngle, utTurnPower * utPowerScale),
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)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("Error occurred on TurnByAngle: " + fault);
}
if (success)
{
LogInfo("BehaviorStraightInterruptTurn_Turn() Finished --------");
}
else
{
LogInfo("BehaviorStraightInterruptTurn_Turn() Canceled --------");
}
yield break;
}
protected IEnumerator <ITask> BehaviorStraightInterruptTurn_Straight()
{
LogInfo("DriveBehaviorServiceBase: BehaviorStraightInterruptTurn_Straight() Started");
bool success = true;
Fault fault = null;
// Drive straight ahead
yield return(Arbiter.Choice(
// 3 meters takes about 30 seconds:
Translate((int)(3.0d * 1000.0d), utForwardVelocity * utPowerScale),
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.
if (success)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
LogError("Error occurred on Translate: " + fault);
}
if (success)
{
LogInfo("BehaviorStraightInterruptTurn_Straight() Finished --------");
}
else
{
LogInfo("BehaviorStraightInterruptTurn_Straight() Canceled --------");
}
yield break;
}
protected IEnumerator <ITask> BackUpTurnIterator(Port <bool> butwCompletionPort, TurnAndMoveParameters tamp)
{
bool lastOpSuccess = true;
Fault fault = null;
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() starting ");
// First backup a little.
yield return(Arbiter.Choice(
Translate(-tamp.distance, tamp.speed),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f) { lastOpSuccess = false; fault = f; }
));
// If the DriveDistance was accepted, then wait for it to complete, and start turn.
if (!lastOpSuccess)
{
LogError("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - Translate FAULT : " + fault);
}
else
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - BackUp portion completed");
if (Math.Abs(tamp.rotateAngle) > 5)
{
// Wait for settling time
//yield return Timeout(settlingTime);
// now we can Turn:
yield return(Arbiter.Choice(
TurnByAngle(tamp.rotateAngle, ModerateTurnPower),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f) { lastOpSuccess = 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 (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true;
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = state.LastTurnCompleted;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - Turn FAULT : " + fault);
}
}
}
else
{
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - BackUp portion canceled");
}
}
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() completed");
butwCompletionPort.Post(true);
// done
yield break;
}
protected IEnumerator <ITask> KataRunner(Kata kata, Handler onComplete)
{
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") Started" + currentCompass);
kata.success = false;
kata.successfulStepsCount = 0;
bool lastOpSuccess = false;
_state.MovingState = MovingState.InTransition;
// onComplete handler may set MovingState to whatever appropriate. We can set MovingState.Unknown on any error or interruption, and at the end to tamp.desiredMovingState
lastInTransitionStarted = DateTime.Now;
foreach (KataStep kataStep in kata)
{
LogInfo("IP: KataRunner(" + kata.name + ") started step " + (kata.successfulStepsCount + 1) + " " + kataStep.name + currentCompass);
kataStep.success = false;
Fault fault = null;
int rotateAngle = kataStep.rotateAngle;
int distance = kataStep.distance;
if (Math.Abs(rotateAngle) > 1) // "rotate" step
{
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on turn");
break; // kata interrupted
}
LogInfo("IP: KataRunner Turn " + rotateAngle);
yield return(Arbiter.Choice(
TurnByAngle(rotateAngle, kataStep.rotatePower * PowerScale),
delegate(DefaultUpdateResponseType response)
{
LogInfo("IP: KataRunner TurnByAngle accepted" + currentCompass);
lastOpSuccess = true;
},
delegate(Fault f)
{
LogInfo("Error: KataRunner TurnByAngle rejected: " + fault);
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
));
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true; // can't trust laser while turning
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("OK: WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogInfo("op failure");
state.MovingState = MovingState.Unknown;
}
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
else
{
break;
}
}
if (Math.Abs(distance) > 1) // "translate" step
{
// while we were rotating, collisionState might have changed
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on translate");
break; // kata interrupted
}
LogInfo("IP: KataRunner Translate " + distance);
yield return(Arbiter.Choice(
Translate(distance, kataStep.speed * PowerScale),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f)
{
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
));
// If the Translate was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
if (!lastOpSuccess)
{
break;
}
}
LogInfo("KataRunner(" + kata.name + ") finished step=" + kataStep.name + currentCompass);
kata.successfulStepsCount++;
}
kata.success = kata.Count == kata.successfulStepsCount;
_state.MovingState = MovingState.Unknown; // that's for now, onComplete may set it to whatever appropriate
LogInfo("KataRunner - calling onComplete()");
onComplete(); // check kata.success, will be false if DriveStage.Cancel or other interruption occured.
// done
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") finished" + currentCompass);
yield break;
}
// 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> BehaviorSquare()
{
LogInfo(LogGroups.Console, "DriveBehaviorServiceBase: BehaviorSquare Started");
Talker.Say(2, "starting Behavior Square");
// 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));
}
LogInfo(LogGroups.Console, "Starting SQUARE using Controlled Moves ...");
// Make sure that the drive is enabled first!
//EnableMotor();
int times = 1;
for (; times <= repeatCount; times++)
{
// Drive along the four sides of a square
for (int side = 0; side < 4; side++)
{
bool success = true;
Fault fault = null;
LogInfo(LogGroups.Console, "Driving Straight Ahead - side " + side);
// Drive straight ahead
yield return(Arbiter.Choice(
Translate((int)(driveDistanceMeters * 1000.0d), utForwardVelocity * utPowerScale),
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)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
LogError("Error occurred on Translate: " + fault);
}
// Wait for settling time
yield return(Timeout(settlingTime));
LogInfo(LogGroups.Console, "Turning " + (rotateAngle > 0.0d ? "Right " : "Left ") + " - side " + side);
// Now turn:
yield return(Arbiter.Choice(
TurnByAngle((int)rotateAngle, utTurnPower * utPowerScale),
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)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("Error occurred on TurnByAngle: " + fault);
}
// Wait for settling time
yield return(Timeout(settlingTime));
}
}
// And finally make sure that the robot is stopped!
//StopMoving();
LogInfo(LogGroups.Console, "BehaviorSquare Finished after completing " + (times - 1) + " cycles; robot stopped");
Talker.Say(2, "Behavior Square finished");
yield break;
}
// 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
protected IEnumerator <ITask> BehaviorStraight()
{
LogInfo("DriveBehaviorServiceBase: BehaviorStraight() Started");
// 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));
}
Talker.Say(2, "starting Behavior Straight");
LogInfo("Starting STRAIGHT using Controlled Moves ...");
// Make sure that the drive is enabled first!
//EnableMotor();
for (int times = 1; times <= repeatCount; times++)
{
// Wait for settling time
yield return(Timeout(settlingTime));
// This code uses the Translate operation 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;
LogInfo("Drive Straight Ahead - starting step " + times);
// Drive straight ahead
yield return(Arbiter.Choice(
Translate((int)(driveDistanceMeters * 1000.0d), utForwardVelocity * utPowerScale),
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)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
LogError("Error occurred on Translate: " + fault);
}
LogInfo("Drive Straight Ahead - finished step " + times);
}
// And finally make sure that the robot is stopped!
//StopMoving();
LogInfo("STRAIGHT Finished, robot stopped");
Talker.Say(2, "Behavior Straight finished");
yield break;
}
// 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
protected IEnumerator <ITask> BehaviorTurnAndMoveForward()
{
LogInfo("DriveBehaviorServiceBase: BehaviorTurnAndMoveForward() Started");
Talker.Say(2, "starting Behavior Turn And Move Forward");
// Wait for settling time
yield return(Timeout(settlingTime));
LogInfo(LogGroups.Console, "Turning " + (rotateAngle > 0.0d ? "Right " : "Left "));
bool success = true;
Fault fault = null;
// First turn:
yield return(Arbiter.Choice(
TurnByAngle((int)rotateAngle, utTurnPower * utPowerScale),
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)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
success = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("Error occurred on TurnByAngle: " + fault);
}
if (success)
{
// Wait for settling time
//yield return Timeout(settlingTime);
double speedMms = utForwardVelocity * utPowerScale;
// a fire-and-forget command to move forward:
SetDriveSpeed(speedMms, speedMms);
// wait some time
for (int i = 10; i > 0; i--)
{
LogInfo(i.ToString());
yield return(Timeout(1000));
}
// we expect the drive to stop at the command, not by completion:
StopMoving();
Talker.Say(2, "Behavior Turn And Move Forward finished");
}
else
{
Talker.Say(2, "Behavior Turn And Move Forward canceled");
}
// done
yield break;
}
protected IEnumerator<ITask> KataRunner(Kata kata, Handler onComplete)
{
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") Started" + currentCompass);
kata.success = false;
kata.successfulStepsCount = 0;
bool lastOpSuccess = false;
_state.MovingState = MovingState.InTransition;
// onComplete handler may set MovingState to whatever appropriate. We can set MovingState.Unknown on any error or interruption, and at the end to tamp.desiredMovingState
lastInTransitionStarted = DateTime.Now;
foreach (KataStep kataStep in kata)
{
LogInfo("IP: KataRunner(" + kata.name + ") started step " + (kata.successfulStepsCount + 1) + " " + kataStep.name + currentCompass);
kataStep.success = false;
Fault fault = null;
int rotateAngle = kataStep.rotateAngle;
int distance = kataStep.distance;
if (Math.Abs(rotateAngle) > 1) // "rotate" step
{
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on turn");
break; // kata interrupted
}
LogInfo("IP: KataRunner Turn " + rotateAngle);
yield return Arbiter.Choice(
TurnByAngle(rotateAngle, kataStep.rotatePower * PowerScale),
delegate(DefaultUpdateResponseType response)
{
LogInfo("IP: KataRunner TurnByAngle accepted" + currentCompass);
lastOpSuccess = true;
},
delegate(Fault f)
{
LogInfo("Error: KataRunner TurnByAngle rejected: " + fault);
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
);
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true; // can't trust laser while turning
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
DriveStageContainer driveStage = new DriveStageContainer();
yield return WaitForCompletion(driveStage);
LogInfo("OK: WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogInfo("op failure");
state.MovingState = MovingState.Unknown;
}
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
else
{
break;
}
}
if (Math.Abs(distance) > 1) // "translate" step
{
// while we were rotating, collisionState might have changed
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on translate");
break; // kata interrupted
}
LogInfo("IP: KataRunner Translate " + distance);
yield return Arbiter.Choice(
Translate(distance, kataStep.speed * PowerScale),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f)
{
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
);
// If the Translate was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return WaitForCompletion(driveStage);
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
if (!lastOpSuccess)
{
break;
}
}
LogInfo("KataRunner(" + kata.name + ") finished step=" + kataStep.name + currentCompass);
kata.successfulStepsCount++;
}
kata.success = kata.Count == kata.successfulStepsCount;
_state.MovingState = MovingState.Unknown; // that's for now, onComplete may set it to whatever appropriate
LogInfo("KataRunner - calling onComplete()");
onComplete(); // check kata.success, will be false if DriveStage.Cancel or other interruption occured.
// done
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") finished" + currentCompass);
yield break;
}
/// <summary>
/// performs a turn and sets the speed before exiting, may leave robot moving straight.
/// exits immediately after the turn is completed (or interrupted).
/// </summary>
/// <param name="tamp">contains turn angle and desired speed</param>
/// <param name="onComplete">handler to be called after the turn is completed and the speed is set</param>
/// <returns></returns>
protected IEnumerator<ITask> TurnAndMoveForward(TurnAndMoveParameters tamp, Handler onComplete)
{
LogInfo("DriveBehaviorServiceBase: TurnAndMoveForward() Started" + currentCompass);
bool lastOpSuccess = true; // must be true in case translate back is not needed
_state.MovingState = MovingState.InTransition;
// onComplete handler may set MovingState to whatever appropriate. We can set MovingState.Unknown on any error or interruption, and at the end to tamp.desiredMovingState
lastInTransitionStarted = DateTime.Now;
Talker.Say(2, "Turn " + (tamp.rotateAngle > 0 ? "Right " : "Left ") + Math.Abs(tamp.rotateAngle) + " And Move Forward");
LogInfo(LogGroups.Console, "Turning " + (tamp.rotateAngle > 0 ? "right " : "left ") + Math.Abs(tamp.rotateAngle) + " degrees, power=" + tamp.rotatePower + " - and move forward at speed=" + tamp.speed);
tamp.success = false;
Fault fault = null;
int rotateAngle = tamp.rotateAngle;
double? desiredHeading = Direction.to360(_mapperVicinity.robotDirection.heading + rotateAngle);
CollisionState collisionState = _state.collisionState;
if (Math.Abs(rotateAngle) > 45 && collisionState != null && collisionState.canMoveBackwards)
{
LogInfo("Steep Turn " + rotateAngle + " - backing up some...");
// back up a bit to allow some front space for the turn:
yield return Arbiter.Choice(
Translate(-backupDistanceSteepTurn, tamp.speed),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f) {
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f; }
);
// If the Translate was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return WaitForCompletion(driveStage);
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (!lastOpSuccess)
{
_state.MovingState = MovingState.Unknown;
}
}
else
{
_state.MovingState = MovingState.Unknown;
LogError("Error occurred in TurnAndMoveForward() on Backup Sharp Turn: " + fault);
}
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
if (lastOpSuccess) // either we didn't need to backup, or backup completed successfully.
{
if (Math.Abs(rotateAngle) > 3)
{
int tryCount = 0;
while (lastOpSuccess && Math.Abs(rotateAngle) > 3 && ++tryCount < 3)
{
LogInfo(LogGroups.Console, "Turning " + (tryCount==1?"start":"retry") + ": current heading=" + _mapperVicinity.robotDirection.heading + " desiredHeading=" + desiredHeading + " rotateAngle=" + rotateAngle);
yield return Arbiter.Choice(
TurnByAngle(rotateAngle, tamp.rotatePower),
delegate(DefaultUpdateResponseType response)
{
LogInfo("success in TurnByAngle()" + currentCompass);
lastOpSuccess = true;
},
delegate(Fault f)
{
LogInfo("fault in TurnByAngle()");
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
);
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true; // can't trust laser while turning
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
DriveStageContainer driveStage = new DriveStageContainer();
yield return WaitForCompletion(driveStage);
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogInfo("op failure");
state.MovingState = MovingState.Unknown;
}
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
if (tamp.speed != 0 && lastOpSuccess)
{
// a fire-and-forget command to move forward:
LogInfo("calling SetDriveSpeed()");
SetDriveSpeed(tamp.speed, tamp.speed);
}
else
{
// make sure we display zero power if SetDriveSpeed() was not called:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
rotateAngle = (int)Direction.to180(desiredHeading - _mapperVicinity.robotDirection.heading);
LogInfo(LogGroups.Console, "Turning end: current heading=" + _mapperVicinity.robotDirection.heading + " desiredHeading=" + desiredHeading + " remains rotateAngle=" + rotateAngle);
}
else
{
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
LogError("Error occurred in TurnAndMoveForward() on TurnByAngle: " + fault);
}
}
}
else
{
// no need to turn (angle too small) - just set the speed:
_state.IsTurning = false;
if (tamp.speed != 0)
{
// a fire-and-forget command to move forward, if speed is specified:
LogInfo("calling SetDriveSpeed()");
SetDriveSpeed(tamp.speed, tamp.speed);
}
}
}
// we come here with MovingState.InTransition or MovingState.Unknown
tamp.success = lastOpSuccess;
_state.MovingState = lastOpSuccess ? tamp.desiredMovingState : MovingState.Unknown; // that's for now, onComplete may set it to whatever appropriate
LogInfo("calling onComplete()");
onComplete(); // check tamp.success, will be false if DriveStage.Cancel or other interruption occured.
// done
yield break;
}
/// <summary>
/// WaitForCompletion - Helper function to wait on Completion Port
/// </summary>
/// <returns>Receiver suitable for waiting on</returns>
public Receiver<drive.DriveStage> WaitForCompletion(DriveStageContainer _driveStage)
{
LogInfo(string.Format("++++++++++++++++++ DriveBehaviorServiceBase:: WaitForCompletion()"));
// Note that this method does nothing with the drive status
//return Arbiter.Receive(false, _completionPort, EmptyHandler<drive.DriveStage>);
//return Arbiter.Receive(false, _completionPort, driveCompletionHandler);
return Arbiter.Receive(false, _completionPort,
delegate(drive.DriveStage driveStage) {
LogInfo("++++++++++++++++++ DriveBehaviorServiceBase:: WaitForCompletion() delegate ---------------------- driveStage=" + driveStage);
_driveStage.DriveStage = driveStage;
}
);
}
// Iterator to execute the turn
// It is important to use an Iterator so that it can relinquish control when there is nothing to do, i.e. yield return
protected IEnumerator<ITask> TurnByDegree(TurnAndMoveParameters tamp, Handler onComplete)
{
LogInfo("DriveBehaviorServiceBase: TurnByDegree() Started");
bool lastOpSuccess = true;
_state.MovingState = MovingState.InTransition;
// onComplete handler may set MovingState to whatever appropriate. We can set MovingState.Unknown on any error or interruption, and at the end to tamp.desiredMovingState
lastInTransitionStarted = DateTime.Now;
LogInfo(LogGroups.Console, "Turning " + (tamp.rotateAngle > 0 ? "right " : "left ") + Math.Abs(tamp.rotateAngle) + " degrees, power=" + tamp.rotatePower);
tamp.success = false;
Fault fault = null;
int rotateAngle = tamp.rotateAngle;
double? desiredHeading = Direction.to360(_mapperVicinity.robotDirection.heading + rotateAngle);
// Turn:
yield return Arbiter.Choice(
TurnByAngle(rotateAngle, tamp.rotatePower),
delegate(DefaultUpdateResponseType response)
{
LogInfo("success in TurnByAngle()" + currentCompass);
lastOpSuccess = true;
},
delegate(Fault f)
{
LogInfo("fault in TurnByAngle()");
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
);
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return WaitForCompletion(driveStage);
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("Error occurred on TurnByAngle: " + fault);
_state.MovingState = MovingState.Unknown;
}
// done
_state.IsTurning = false;
_state.LastTurnCompleted = DateTime.Now;
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
yield break;
}
