protected void BumperHandler(bumper.Update notification) { if (string.IsNullOrEmpty(notification.Body.Name)) { LogInfo(LogGroups.Console, "Bumper name is null or empty."); return; } string bumperName = notification.Body.Name.ToLowerInvariant(); //Trace.WriteLine(bumperName + " @ " + DateTime.Now + ": " + notification.Body.Pressed); if (bumperName.Contains("front")) { lock (_soar.Bumper) { _soar.Bumper.FrontBumperPressed = notification.Body.Pressed; } } else if (bumperName.Contains("rear")) { lock (_soar.Bumper) { _soar.Bumper.RearBumperPressed = notification.Body.Pressed; } } }
/// <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; }
public virtual IEnumerator<ITask> ReliableSubscribeHandler(contactsensor.ReliableSubscribe subscribe) { yield return (Choice)base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort); }
public virtual IEnumerator<ITask> GenericSubscribeHandler(bumper.Subscribe subscribe) { base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort); yield break; }
/// <summary> /// Update the UI with the estat of a single bumper /// </summary> /// <param name="bumperState"></param> public void UpdateBumperData(pxbumper.ContactSensor bumperState) { UpdateBumperButton(bumperState.Name, bumperState.Pressed); }
/// <summary> /// Handles Update notification from the Bumper partner /// </summary> /// <remarks>Posts a <typeparamref name="BumperUpdate"/> to itself.</remarks> /// <param name="update">notification</param> void BumperUpdateNotification(bumper.Update update) { LogInfo("Explorer: BumperUpdateNotification()"); _mainPort.Post(new BumperUpdate(update.Body)); }
public virtual IEnumerator<ITask> ReplaceHandler(bumper.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }
/// <summary> /// Handles bumper update notification /// </summary> /// <param name="update">bumper update notification</param> /// <returns></returns> IEnumerator<ITask> OnBumperUpdateHandler(pxbumper.Update update) { if (_driveControl != null) { WinFormsServicePort.FormInvoke( delegate() { _driveControl.UpdateBumperData(update.Body); } ); } yield break; }
public virtual IEnumerator<ITask> ReliableSubscribeHandler(pxbumper.ReliableSubscribe subscribe) { LogInfo("TrackRoamerBumper received Reliable Subscription request from Subscriber=" + subscribe.Body.Subscriber); base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort); yield break; }
// Robotics Tutorial 1 Step 5 -- Create a handler for the subscription // The Robotics Tutorial says to add this at the end // of the file, but I don't know why because it compiles // wherever you put it ... // There seems to be a major problem with the approach in // Robotics Tutorial 3. It results in the robot jerking up // against the wall because of a backlog of queued messages. /// <summary> /// Handle Bumper Notifications /// </summary> /// <param name="notification"></param> void BumperHandler(bumper.Update notification) { string message; string bumperName; DateTime thisTimestamp; TimeSpan timediff; // Find out which bumper this is // TT - Nov CTP // int num = notification.Body.Identifier; int num = notification.Body.HardwareIdentifier; // Robotics Tutorial 3: //contactsensor.ContactSensor s = updateNotification.Body; if (string.IsNullOrEmpty(notification.Body.Name)) bumperName = ""; else bumperName = notification.Body.Name.ToLowerInvariant(); if (!notification.Body.Pressed) { message = "Bumper " + num.ToString() + " (" + bumperName + ") was released."; LogInfo(LogGroups.Console, message); return; } else { message = "Ouch! Bumper " + num.ToString() + " (" + bumperName + ") was pressed."; LogInfo(LogGroups.Console, message); StopNow(); } // OK, we have a bump! // What if it is the OPPOSITE bumper? // This should be handled better. // In particular, there are instances where both front // AND back bumpers are triggered! It would be a better // idea to remember the direction of motion and always // back up when a bumper notification is received. // Get the current time thisTimestamp = System.DateTime.Now; // Make sure that we have waited long enough for the last // motion to complete. This is a fudge that effectively // renders the robot blind for a short period of time. // Not really a good solution if there is a chance that // the opposite bumper will be triggered legitimately. // It is sometimes referred to as a "dead zone" and is // similar to what is used in simple "bang bang" or // "on/off" control to avoid the continuous oscillations. if (thisTimestamp < _state.nextTimestamp) { int count = 0; bumper.Replace testBump = null; bumper.Replace testBump2 = null; int skip = bumperNotificationPort.P3.ItemCount; // We don't want any more bump notifications while we // are processing the current one. // This code for skipping notifications uses a pattern // from the Traxster motor.cs. However, the types of // messages involved are different so it is not just a // copy and paste :-( // Note that the queue can easily get over a thousand // messages in a short space of time! for (int i = 0; i < skip; i++) { testBump = (bumper.Replace) bumperNotificationPort.P3.Test(); if (testBump != null) { count++; // reply and pretend everything went well testBump.ResponsePort.Post(DefaultReplaceResponseType.Instance); } } if (count > 0) { message = "Ignored " + num.ToString() + " - " + count.ToString() + " messages"; // LogInfo(LogGroups.Console, message); } // Return without doing anything return; } // Wait for the motion to complete for this amount // of time, i.e. ignore future bumps!!! // This is a fudge because we don't know for sure // how long any particular motion will take, but // it's the best we can do. _state.nextTimestamp = thisTimestamp.AddMilliseconds(3500); // From Robotics Tutorial 3 // Look carefully at the code. At first glance, it seems // to call the same function for either bumper, but the // polarity parameter is different. if (bumperName.Contains("front")) { if (_state.insideBehavior <= 0) { LogInfo(LogGroups.Console, ">>> Start -1"); // Set a flag so that multiple threads // will not execute the behavior at the // same time. This is possible, but the // motor commands will get "interleaved" // and the robot does some really strange // things! _state.insideBehavior++; // Create a new iterator to execute the // behavior SpawnIterator<double>(-1, BackUpTurnAndMove); } return; } if (bumperName.Contains("rear")) { if (_state.insideBehavior <= 0) { LogInfo(LogGroups.Console, ">>> Start +1"); // Set the synchronization flag _state.insideBehavior++; SpawnIterator<double>(1, BackUpTurnAndMove); } return; } }
/// <summary> /// Handles Update notification from the Bumper partner /// </summary> /// <remarks>Posts a <typeparamref name="BumperUpdate"/> to itself.</remarks> /// <param name="update">notification</param> void BumperUpdateNotification(bumper.Update update) { _mainPort.Post(new BumperUpdate(update.Body)); }
/// <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); }
public virtual IEnumerator<ITask> GetContatSensorHandler(contactsensor.Get get) { get.ResponsePort.Post(SyncGenericContactState()); yield break; }
private bool isSensingRearObstruction(bool leftWhiskerPressed, bool rightWhiskerPressed, bumper.ContactSensorArrayState bumpersState) { return !leftWhiskerPressed && !rightWhiskerPressed; }
public IEnumerator<ITask> ReliableSubscribeHandler(pxContactSensor.ReliableSubscribe subscribe) { yield return Arbiter.Choice( SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort), delegate(SuccessResult success) { _subMgrPort.Post(new submgr.Submit( subscribe.Body.Subscriber, dssp.DsspActions.ReplaceRequest, _state, null)); }, delegate(Exception ex) { LogError(ex); } ); }
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; }
public IEnumerator<ITask> GetHandler(pxContactSensor.Get get) { get.ResponsePort.Post(_state); yield break; }
public virtual IEnumerator<ITask> GenericUpdateHandler(bumper.Update update) { throw new InvalidOperationException("Contact Sensor Update is a Notification and not valid in this context."); }
/// <summary> /// Handles Replace notifications from the Bumper partner /// </summary> /// <remarks>Posts a <typeparamref name="BumpersUpdate"/> to itself.</remarks> /// <param name="replace">notification</param> void BumperReplaceNotification(bumper.Replace replace) { _mainPort.Post(new BumpersUpdate(replace.Body)); }
public virtual IEnumerator<ITask> ReplaceHandler(contactsensor.Replace replace) { replace.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The LEGO NXT touch sensor is updated from hardware."))); yield break; }
/// <summary> /// Handlers Bumpers Replace notification /// </summary> /// <param name="replace">Bumpers replace notification</param> /// <returns></returns> IEnumerator<ITask> OnBumpersReplaceHandler(pxbumper.Replace replace) { if (_driveControl != null) { WinFormsServicePort.FormInvoke( delegate() { _driveControl.ReplaceBumpersData(replace.Body); } ); } LogObject(replace.Body); yield break; }
public virtual IEnumerator<ITask> SubscribeHandler(contactsensor.Subscribe subscribe) { yield return (Choice)base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort); if (_state.TimeStamp != DateTime.MinValue) { SendNotificationToTarget<contactsensor.Replace>(subscribe.Body.Subscriber, _genericSubMgrPort, SyncGenericContactState()); } }
public virtual IEnumerator<ITask> UpdateHandler(pxbumper.Update updateBumper) { throw new InvalidOperationException("Track Roamer Bumper Sensors are not updateable"); }
/// <summary> /// Handles Update notification from the Bumper partner - updates a single sensor state (usually whiskers) /// </summary> /// <remarks>Posts a <typeparamref name="BumperUpdate"/> to itself.</remarks> /// <param name="update">notification</param> void BumperUpdateNotification(bumper.Update update) { LogInfo("DriveBehaviorServiceBase: BumperUpdateNotification()"); _mainPort.Post(new BumperUpdate(update.Body)); }
/// <summary> /// Handles Replace notifications from the Bumper partner /// </summary> /// <remarks>Posts a <typeparamref name="BumpersUpdate"/> to itself.</remarks> /// <param name="replace">notification</param> void BumperReplaceNotification(bumper.Replace replace) { LogInfo("Explorer: BumperReplaceNotification()"); _mainPort.Post(new BumpersUpdate(replace.Body)); }
/// <summary> /// Handles Replace notifications from the Bumper partner - updating state of the whole set of sensors /// </summary> /// <remarks>Posts a <typeparamref name="BumpersArrayUpdate"/> to itself.</remarks> /// <param name="replace">notification</param> void BumperReplaceNotification(bumper.Replace replace) { LogInfo("DriveBehaviorServiceBase: BumperReplaceNotification()"); _mainPort.Post(new BumpersArrayUpdate(replace.Body)); }
public virtual IEnumerator<ITask> GetHandler(bumper.Get get) { get.ResponsePort.Post(_state); yield break; }
// Raul - Bumpers Update code #region Bumpers Update /// <summary> /// Update the UI with the state of bumpers /// </summary> /// <param name="bumpersState"></param> public void ReplaceBumpersData(pxbumper.ContactSensorArrayState bumpersState) { foreach (pxbumper.ContactSensor bumper in bumpersState.Sensors) { UpdateBumperButton(bumper.Name, bumper.Pressed); } }
public IEnumerator<ITask> ReliableSubscribeHandler(bumper.ReliableSubscribe subscribe) { yield return Arbiter.Choice( SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort), delegate(SuccessResult success) { _subMgrPort.Post(new submgr.Submit( subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null)); }, null ); }
public virtual IEnumerator<ITask> GenericReplaceHandler(bumper.Replace replace) { throw new InvalidOperationException("The LEGO NXT Contact Sensor Array is configured by the LEGO NXT Brick service."); }