protected void ManualControl(SensorEventSource why)
{
if (!prevManualControl)
{
stopCount = 5;
lastStopCount = DateTime.MinValue;
}
string command = _mapperVicinity.robotState.manualControlCommand;
if (!string.IsNullOrEmpty(command))
{
Tracer.Trace("on manual control; command: " + command);
if ("stop".Equals(command))
{
stopCount = 5;
lastStopCount = DateTime.MinValue;
}
else if ("turn".Equals(command))
{
int bestHeading = (int)_mapperVicinity.robotDirection.turnRelative; // right - positive, left - negative
_state.NewHeading = bestHeading;
LogInfo(" bestHeading=" + bestHeading + currentCompass + " (relative=" + bestHeading + ")");
rotateAngle = bestHeading;
SpawnIterator <TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor),
rotatePower = ModerateTurnPower,
speed = 0,
desiredMovingState = MovingState.LayingDown
},
delegate()
{
},
TurnByDegree);
//TurnAndMoveForward);
}
_mapperVicinity.robotState.manualControlCommand = string.Empty;
}
else if (why == SensorEventSource.LaserScanning)
{
Tracer.Trace("on manual control; listening");
}
}
protected void ManualControl(SensorEventSource why)
{
if (!prevManualControl)
{
stopCount = 5;
lastStopCount = DateTime.MinValue;
}
string command = _mapperVicinity.robotState.manualControlCommand;
if (!string.IsNullOrEmpty(command))
{
Tracer.Trace("on manual control; command: " + command);
if ("stop".Equals(command))
{
stopCount = 5;
lastStopCount = DateTime.MinValue;
}
else if ("turn".Equals(command))
{
int bestHeading = (int)_mapperVicinity.robotDirection.turnRelative; // right - positive, left - negative
_state.NewHeading = bestHeading;
LogInfo(" bestHeading=" + bestHeading + currentCompass + " (relative=" + bestHeading + ")");
rotateAngle = bestHeading;
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor),
rotatePower = ModerateTurnPower,
speed = 0,
desiredMovingState = MovingState.LayingDown
},
delegate()
{
},
TurnByDegree);
//TurnAndMoveForward);
}
_mapperVicinity.robotState.manualControlCommand = string.Empty;
}
else if (why == SensorEventSource.LaserScanning)
{
Tracer.Trace("on manual control; listening");
}
}
/// <summary>
/// called often, by the timer and any state change or incoming sensor notifications;
/// make all decisions about immediate actions here. Normal decision making happens in DecisionMainLoop()
/// </summary>
/// <param name="why">0-laser, 1-Accelerometer, 2-Direction, 3-Proximity</param>
protected void Decide(SensorEventSource why)
{
if (_state.collisionState == null)
{
_state.collisionState = new CollisionState();
}
if (_doDecisionDontMove)
{
return;
}
if (_mapperVicinity.robotState.manualControl)
{
ManualControl(why);
prevManualControl = true;
doStop(); // will only stop if stopCount > 0
return;
}
prevManualControl = false;
doStop(); // will only stop if stopCount > 0
//LogInfo("TrackRoamerBehaviorsService: Decide() MovingState=" + _state.MovingState);
//Tracer.Trace("TrackRoamerBehaviorsService: Decide() MovingState=" + _state.MovingState + " IsTurning=" + _state.IsTurning);
//_laserData = simulatedLaser(); // you can call it here as well, so that real data frame, when it comes, is just replaced on the fly
// consider resetting some state flags, if timing calls for it:
// if the flag indicates Turning, but it's been a while since we initiated the turn - reset the flag:
if (_state.IsTurning && ((DateTime.Now - _state.LastTurnCompleted).TotalSeconds > TurningWatchdogInterval || (DateTime.Now - _state.LastTurnStarted).TotalSeconds > TurningWatchdogInterval))
{
LogInfo("Decide(): reset isTurning flag");
LogHistory(10, "Turning took too long");
_state.IsTurning = false;
}
if (_state.MovingState == MovingState.BumpedBackingUp && (DateTime.Now - lastBumpedBackingUpStarted).TotalSeconds > TurningWatchdogInterval)
{
LogInfo("Decide(): reset MovingState.BumpedBackingUp to Unknown");
LogHistory(10, "BumpedBackingUp took too long");
_state.MovingState = MovingState.Unknown;
}
if (_state.MovingState == MovingState.InTransition && (DateTime.Now - lastInTransitionStarted).TotalSeconds > InTransitionWatchdogInterval)
{
LogInfo("Decide(): reset MovingState.InTransition to Unknown");
LogHistory(10, "InTransition took too long");
_state.MovingState = MovingState.Unknown;
}
// unit test behavior - just go forward
if (_doDecisionStraightForward)
{
if (_state.MovingState != MovingState.BumpedBackingUp && _state.MovingState != MovingState.InTransition)
{
LogHistory(10, "Decisions Disabled - FreeForwards");
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
speed = (int)Math.Round(ModerateForwardVelocityMmSec),
rotatePower = ModerateTurnPower,
desiredMovingState = MovingState.FreeForwards
},
delegate()
{
},
TurnAndMoveForward);
}
return;
}
}
/// <summary>
/// called often, by the timer and any state change or incoming sensor notifications;
/// make all decisions about immediate actions here. Normal decision making happens in DecisionMainLoop()
/// </summary>
/// <param name="why">0-laser, 1-Accelerometer, 2-Direction, 3-Proximity</param>
protected void Decide(SensorEventSource why)
{
if (_state.collisionState == null)
{
_state.collisionState = new CollisionState();
}
if (_doDecisionDontMove)
{
return;
}
if (_mapperVicinity.robotState.manualControl)
{
ManualControl(why);
prevManualControl = true;
doStop(); // will only stop if stopCount > 0
return;
}
prevManualControl = false;
doStop(); // will only stop if stopCount > 0
//LogInfo("TrackRoamerBehaviorsService: Decide() MovingState=" + _state.MovingState);
//Tracer.Trace("TrackRoamerBehaviorsService: Decide() MovingState=" + _state.MovingState + " IsTurning=" + _state.IsTurning);
//_laserData = simulatedLaser(); // you can call it here as well, so that real data frame, when it comes, is just replaced on the fly
// consider resetting some state flags, if timing calls for it:
// if the flag indicates Turning, but it's been a while since we initiated the turn - reset the flag:
if (_state.IsTurning && ((DateTime.Now - _state.LastTurnCompleted).TotalSeconds > TurningWatchdogInterval || (DateTime.Now - _state.LastTurnStarted).TotalSeconds > TurningWatchdogInterval))
{
LogInfo("Decide(): reset isTurning flag");
LogHistory(10, "Turning took too long");
_state.IsTurning = false;
}
if (_state.MovingState == MovingState.BumpedBackingUp && (DateTime.Now - lastBumpedBackingUpStarted).TotalSeconds > TurningWatchdogInterval)
{
LogInfo("Decide(): reset MovingState.BumpedBackingUp to Unknown");
LogHistory(10, "BumpedBackingUp took too long");
_state.MovingState = MovingState.Unknown;
}
if (_state.MovingState == MovingState.InTransition && (DateTime.Now - lastInTransitionStarted).TotalSeconds > InTransitionWatchdogInterval)
{
LogInfo("Decide(): reset MovingState.InTransition to Unknown");
LogHistory(10, "InTransition took too long");
_state.MovingState = MovingState.Unknown;
}
// unit test behavior - just go forward
if (_doDecisionStraightForward)
{
if (_state.MovingState != MovingState.BumpedBackingUp && _state.MovingState != MovingState.InTransition)
{
LogHistory(10, "Decisions Disabled - FreeForwards");
SpawnIterator <TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
speed = (int)Math.Round(ModerateForwardVelocityMmSec),
rotatePower = ModerateTurnPower,
desiredMovingState = MovingState.FreeForwards
},
delegate()
{
},
TurnAndMoveForward);
}
return;
}
}
