/// <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>
/// Stops the robot. If the robot was going forward it backs up.
/// Keep in mind that a "hardware" stop has already been initiated in the Power Brick, so by the time we get here wheels are stopped.
/// </summary>
protected void Bumped(bool leftWhiskerPressed, bool rightWhiskerPressed, bumper.ContactSensorArrayState bumpersState)
{
LogInfo("TrackRoamerBehaviorsService: Bumped() _state.Velocity=" + _state.Velocity + " _state.MovingState=" + _state.MovingState);
string whatIsBumped;
int turnFactor; // = btRand.NextDouble() > 0.5d ? 1 : -1;
if (leftWhiskerPressed && rightWhiskerPressed)
{
whatIsBumped = "Both whiskers";
turnFactor = 0; // straight back
}
if (leftWhiskerPressed)
{
whatIsBumped = "Left whisker";
turnFactor = -1; // turning a bit to the right, to avoid obstacle to the left
}
else if (rightWhiskerPressed)
{
whatIsBumped = "Right whisker";
turnFactor = 1; // turning a bit to the left, to avoid obstacle to the right
}
else
{
whatIsBumped = "Proximity array";
turnFactor = btRand.NextDouble() > 0.5d ? 1 : -1;
}
Talker.Say(3, "bumped " + whatIsBumped);
if (!_testBumpMode && _state.Velocity < 0)
{
// we are moving backwards,
// front whiskers ignored when we move backwards. Others cause immediate stop:
if (isSensingRearObstruction(leftWhiskerPressed, rightWhiskerPressed, bumpersState))
{
LogInfo("TrackRoamerBehaviorsService: Bumped() - only Rear proximity sensors pressed while moving backwards, stopping...");
// either a rear bumper or both front and rear
// bumpers are pressed. STOP!
StopTurning();
//StopMoving();
// whatever it was, we didn't expect it. Let higher level decision-making take over, may be look around, do mapping:
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
else
{
LogInfo("TrackRoamerBehaviorsService: Bumped() while moving backwards, whisker press ignored (robot stopped anyway)");
Talker.Say(4, whatIsBumped + " ignored");
// well, motors are stopped and we are not getting completion by encoders.
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
}
else
{
// we are moving forward, or in test mode.
// _testBumpMode always ends here - even if we are stationary or moving backwards
if (_state.MovingState != MovingState.BumpedBackingUp)
{
_state.MovingState = MovingState.BumpedBackingUp;
lastBumpedBackingUpStarted = DateTime.Now;
int angle = BackupAngleDegrees * turnFactor;
Tracer.Trace("TrackRoamerBehaviorsService: Bumped() - " + whatIsBumped + " pressed, backing up by " + (-BackupDistanceMm) + " mm turning " + angle + " degrees");
Talker.Say(4, "backing up");
// only a front bumper is pressed.
// move back <BackupDistance> mm;
TurnAndMoveParameters tamp = new TurnAndMoveParameters()
{
distance = BackupDistanceMm,
speed = (int)Math.Round(ModerateBackwardVelocityMmSec),
rotatePower = ModerateTurnPower,
rotateAngle = angle
};
Port<bool> completionPort = BackUpTurnWait(tamp);
// start movement
Activate(Arbiter.Receive(false, completionPort,
delegate(bool b)
{
LogInfo("TrackRoamerBehaviorsService: Bumped() delegate - ++++++++++++ BackUpTurn done ++++++++++++++++++++++++++++++++++++++++++++++++++++");
Talker.Say(4, "done backing up");
// done backing up; let the decision making process take over:
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
)
);
// exiting here with MovingState.BumpedBackingUp, while the delegate waits for completion.
}
else
{
Talker.Say(4, "whisker press ignored");
// well, motors are stopped and we are not getting completion by encoders.
_state.MovingState = MovingState.Unknown;
_state.Countdown = 3;
}
}
LogInfo("TrackRoamerBehaviorsService: Bumped() exiting - _state.MovingState=" + _state.MovingState);
}
// 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;
}
