/// <summary> /// Initialize State /// </summary> private void InitializeState() { if (_state == null) { _state = new TouchSensorState(); } if (_state.PollingFrequencyMs == 0) { _state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs; } _state.TimeStamp = DateTime.MinValue; _state.Connected = false; _state.TouchSensorOn = false; if (_genericContactState == null) { _genericContactState = new contactsensor.ContactSensorArrayState(); } if (_genericContactState.Sensors == null) { _genericContactState.Sensors = new List <contactsensor.ContactSensor>(); } if (_genericContactState.Sensors.Count == 0) { _genericContactState.Sensors.Add(new contactsensor.ContactSensor(0, "Touch Sensor")); } }
private void ValidateState() { //configure initial state if (_state == null) { _state = new bumper.ContactSensorArrayState(); } if (_state.Sensors == null) { _state.Sensors = new List <bumper.ContactSensor>(); } if (_state.Sensors.Count == 0) { _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.LeftBumper, "Front Left Bumper")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.RightBumper, "Front Right Bumper")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.Wall, "Wall")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.VirtualWall, "Virtual Wall")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffLeft, "Cliff Left")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffFrontLeft, "Cliff Front Left")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffFrontRight, "Cliff Front Right")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.CliffRight, "Cliff Right")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.WheelDropLeft, "Left Wheel Drop")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.WheelDropRight, "Right Wheel Drop")); _state.Sensors.Add(new bumper.ContactSensor((int)RoombaBumpers.WheelDropRear, "Rear Wheel Drop")); _state.Sensors.Add(new bumper.ContactSensor(99, "Wheels Stalled")); SaveState(_state); } }
/// <summary> /// Service Start /// </summary> protected override void Start() { //configure default state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List <bumper.ContactSensor>(); bumper.ContactSensor leftBumper = new bumper.ContactSensor(); leftBumper.Name = "FrontLeftIR"; //note: name must contain 'front' for tutorial3 bumper.ContactSensor rightBumper = new bumper.ContactSensor(); rightBumper.Name = "FrontRightIR"; _state.Sensors.Add(leftBumper); _state.Sensors.Add(rightBumper); } // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); SubscribeToScribblerBase(); }
public IEnumerator <ITask> ReplaceHandler(pxContactSensor.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance); _subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest)); yield break; }
/// <summary> /// Service Start /// </summary> protected override void Start() { //configure default state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List<bumper.ContactSensor>(); bumper.ContactSensor leftBumper = new bumper.ContactSensor(); leftBumper.Name = "FrontLeftIR"; //note: name must contain 'front' for tutorial3 bumper.ContactSensor rightBumper = new bumper.ContactSensor(); rightBumper.Name = "FrontRightIR"; _state.Sensors.Add(leftBumper); _state.Sensors.Add(rightBumper); } // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); SubscribeToScribblerBase(); }
public virtual IEnumerator <ITask> RemoveDeviceConfigurationHandler(ResetConfiguration resetConfiguration) { // Clear Runtime Configuration UnsubscribeAllSensors(); _state.SensorConfiguration = new List <SensorConfiguration>(); _state.RuntimeConfiguration = null; _contactSensorArrayState = new bumper.ContactSensorArrayState(); resetConfiguration.ResponsePort.Post(DefaultDeleteResponseType.Instance); yield break; }
/// <summary> /// Checks whether at least one of the contact sensors is pressed. /// </summary> /// <param name="bumpers"><code>true</code> if at least one bumper in <paramref name="bumpers"/> is pressed, otherwise <code>false</code></param> /// <returns></returns> private bool BumpersPressed(bumper.ContactSensorArrayState bumpers) { if (bumpers.Sensors == null) { return(false); } foreach (bumper.ContactSensor s in bumpers.Sensors) { if (s.Pressed) { return(true); } } return(false); }
protected override void Start() { //configure initial state if (_state == null) { LogInfo("TrackRoamerBumper:Start(): _state == null - initializing..."); _state = new pxbumper.ContactSensorArrayState(); _state.Sensors = new List<pxbumper.ContactSensor>(); pxbumper.ContactSensor leftBumper = new pxbumper.ContactSensor(); leftBumper.HardwareIdentifier = 101; leftBumper.Name = "Front Whisker Left"; _state.Sensors.Add(leftBumper); pxbumper.ContactSensor rightBumper = new pxbumper.ContactSensor(); rightBumper.HardwareIdentifier = 201; rightBumper.Name = "Front Whisker Right"; _state.Sensors.Add(rightBumper); SaveState(_state); } else { LogInfo("TrackRoamerBumper:Start(): _state is supplied by file: " + _configFile); } base.Start(); MainPortInterleave.CombineWith( new Interleave( new TeardownReceiverGroup(), new ExclusiveReceiverGroup( Arbiter.Receive<powerbrick.UpdateWhiskers>(true, notificationPortWhiskers, WhiskersNotificationHandler) ), new ConcurrentReceiverGroup() ) ); // display HTTP service Uri LogInfo("TrackRoamerBumper:Start() Service URI=" + ServiceInfo.HttpUri()); // Subscribe to the Hardware Controller for bumper notifications SubscribeToTrackRoamerBot(); }
/// <summary> /// Inspect the ContactSensorArrayState to make sure it /// exists and that each sensor is associated with a valid /// LEGO sensor port (1-4). /// </summary> /// <param name="state"></param> /// <returns></returns> private static bool ValidState(bumper.ContactSensorArrayState state) { if (state != null) { if (state.Sensors != null) { foreach (bumper.ContactSensor sensor in state.Sensors) { if (sensor.HardwareIdentifier < 1 || sensor.HardwareIdentifier > 4) { return(false); } } return(true); } } return(false); }
/// <summary> /// Validate the State. /// </summary> private void ValidateState() { bool saveState = false; if (_state == null) { saveState = true; _state = new NxtContactSensorArrayState(); } if (_state.SensorConfiguration == null) { saveState = true; _state.SensorConfiguration = new List <SensorConfiguration>(); } if (_state.SensorConfiguration.Count == 0) { saveState = true; _state.SensorConfiguration.Add(new SensorConfiguration("Pressed", touch.Contract.DeviceModel, string.Empty, 1.0, 1.0)); _state.SensorConfiguration.Add(new SensorConfiguration("Near", sonar.Contract.DeviceModel, string.Empty, 0.0, 20.0)); _state.SensorConfiguration.Add(new SensorConfiguration("Far", sonar.Contract.DeviceModel, string.Empty, 220.0, 255.0)); } // Clear Runtime Configuration _state.RuntimeConfiguration = null; UnsubscribeAllSensors(); // Clear generic state if (_contactSensorArrayState == null) { _contactSensorArrayState = new bumper.ContactSensorArrayState(); } if (_contactSensorArrayState.Sensors == null) { _contactSensorArrayState.Sensors = new List <bumper.ContactSensor>(); } if (saveState) { SaveState(_state); } // Initialize Runtime Configuration _state.RuntimeConfiguration = new Dictionary <SensorRange, PortConfiguration>(); }
protected override void Start() { //configure initial state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List <contactsensorbase.ContactSensor>(); contactsensorbase.ContactSensor defaultBumper = new contactsensorbase.ContactSensor(); defaultBumper.HardwareIdentifier = 1; defaultBumper.Name = "Default Bumper 1"; _state.Sensors.Add(defaultBumper); SaveState(_state); } Port <bool> successPort = new Port <bool>(); SpawnIterator(successPort, ConfigureHardware); Activate(Arbiter.Receive(false, successPort, delegate(bool success) { if (!success) { LogError("Surveyor SRV-1 Service failed to start. Shutting down."); Shutdown(); } } )); // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); }
/// <summary> /// Service Start /// </summary> protected override void Start() { //configure default state if (_state == null) { _state = new bumper.ContactSensorArrayState(); _state.Sensors = new List <bumper.ContactSensor>(); //A note on these names: //the bumper names must contain 'front' for RoboticsTutorial3 to work //the bumpers should be labeled left and right for the Scribbler Wander service to work //also for the SensorNotificationHandler below //If multiple words, should be spaced properly so TTS can say it right bumper.ContactSensor leftBumper = new bumper.ContactSensor(); leftBumper.Name = "Front Left"; bumper.ContactSensor rightBumper = new bumper.ContactSensor(); rightBumper.Name = "Front Right"; bumper.ContactSensor stallSensor = new bumper.ContactSensor(); stallSensor.Name = "Stall"; _state.Sensors.Add(leftBumper); _state.Sensors.Add(rightBumper); _state.Sensors.Add(stallSensor); } // Listen on the main port for requests and call the appropriate handler. ActivateDsspOperationHandlers(); // Publish the service to the local Node Directory DirectoryInsert(); // display HTTP service Uri LogInfo(LogGroups.Console, "Service uri: "); SubscribeToScribblerBase(); }
public virtual IEnumerator<ITask> RemoveDeviceConfigurationHandler(ResetConfiguration resetConfiguration) { // Clear Runtime Configuration UnsubscribeAllSensors(); _state.SensorConfiguration = new List<SensorConfiguration>(); _state.RuntimeConfiguration = null; _contactSensorArrayState = new bumper.ContactSensorArrayState(); resetConfiguration.ResponsePort.Post(DefaultDeleteResponseType.Instance); yield break; }
public virtual IEnumerator<ITask> ReplaceHandler(bumper.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }
public BumpersArrayUpdate(bumper.ContactSensorArrayState body) : base(body) { }
public virtual IEnumerator <ITask> ReplaceHandler(bumper.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }
public IEnumerator<ITask> ReplaceHandler(pxContactSensor.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance); _subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest)); yield break; }
/// <summary> /// Initialize State /// </summary> private void InitializeState() { if (_state == null) _state = new TouchSensorState(); if (_state.PollingFrequencyMs == 0) _state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs; _state.TimeStamp = DateTime.MinValue; _state.Connected = false; _state.TouchSensorOn = false; if (_genericContactState == null) { _genericContactState = new contactsensor.ContactSensorArrayState(); } if (_genericContactState.Sensors == null) { _genericContactState.Sensors = new List<contactsensor.ContactSensor>(); } if (_genericContactState.Sensors.Count == 0) { _genericContactState.Sensors.Add(new contactsensor.ContactSensor(0, "Touch Sensor")); } }
/// <summary> /// Validate the State. /// </summary> private void ValidateState() { bool saveState = false; if (_state == null) { saveState = true; _state = new NxtContactSensorArrayState(); } if (_state.SensorConfiguration == null) { saveState = true; _state.SensorConfiguration = new List<SensorConfiguration>(); } if (_state.SensorConfiguration.Count == 0) { saveState = true; _state.SensorConfiguration.Add(new SensorConfiguration("Pressed", touch.Contract.DeviceModel, string.Empty, 1.0, 1.0)); _state.SensorConfiguration.Add(new SensorConfiguration("Near", sonar.Contract.DeviceModel, string.Empty, 0.0, 20.0)); _state.SensorConfiguration.Add(new SensorConfiguration("Far", sonar.Contract.DeviceModel, string.Empty, 220.0, 255.0)); } // Clear Runtime Configuration _state.RuntimeConfiguration = null; UnsubscribeAllSensors(); // Clear generic state if (_contactSensorArrayState == null) _contactSensorArrayState = new bumper.ContactSensorArrayState(); if (_contactSensorArrayState.Sensors == null) _contactSensorArrayState.Sensors = new List<bumper.ContactSensor>(); if (saveState) SaveState(_state); // Initialize Runtime Configuration _state.RuntimeConfiguration = new Dictionary<SensorRange, PortConfiguration>(); }
private bool isSensingRearObstruction(bool leftWhiskerPressed, bool rightWhiskerPressed, bumper.ContactSensorArrayState bumpersState) { return(!leftWhiskerPressed && !rightWhiskerPressed); }
/// <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); }