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