/// <summary> /// Handle sensor update message from NXT /// </summary> public void SensorNotificationHandler(legoNXT.Configure notify) { if (notify == null) { throw new ArgumentNullException("notify"); } //update time _state.TimeStamp = DateTime.Now; int newReading = notify.Body.MotorSensorPort[_state.HardwareIdentifier - 1]; //increment counter (and account for any missing ticks if (Math.Abs(newReading - _state.CurrentReading) <= 3) { _state.TicksSinceReset += Math.Abs(newReading - _state.CurrentReading); } else { _state.TicksSinceReset++; } //Update angle _state.CurrentReading = newReading; //notify subscribers on any state change encoder.UpdateTickCount updateHeader = new encoder.UpdateTickCount(new encoder.UpdateTickCountRequest(DateTime.Now, _state.TicksSinceReset)); SendNotification <encoder.UpdateTickCount>(_subMgrPort, updateHeader); }
void StopMotorWithEncoderHandler(encoder.EncoderOperations encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort) { //LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt); int stopWheelAt; bool ignore; switch (side) { case "left": stopWheelAt = stopLeftWheelAt; ignore = !_leftEncoderTickEnabled; break; default: case "right": stopWheelAt = stopRightWheelAt; ignore = !_rightEncoderTickEnabled; break; } if (!ignore && update.Body.Count >= stopWheelAt) { switch (side) { case "left": _leftEncoderTickEnabled = false; break; default: case "right": _rightEncoderTickEnabled = false; break; } // whatever else got stuck there, we are not interested. Keep the port clear. //Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)]; //port.Clear(); motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 0 }); motorPort.Post(stop); Arbiter.Choice(stop.ResponsePort, delegate(DefaultUpdateResponseType resp) { LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); }, delegate(Fault fault) { LogError(fault); } ); LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending internal DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); completionPort.Post(drive.DriveStage.Completed); } }
/// <summary> /// Handle periodic sensor readings from the pxbrick /// </summary> /// <param name="update"></param> private IEnumerator<ITask> NotificationHandler(brick.LegoSensorUpdate update) { LegoResponseGetOutputState outputState = new LegoResponseGetOutputState(update.Body.CommandData); if (outputState.Success && _state.CurrentEncoderTimeStamp < outputState.TimeStamp) { LogVerbose(LogGroups.Console, outputState.ToString()); int reversePolaritySign = (_state.ReversePolarity) ? -1 : 1; _rpmCalcPort.Post(outputState); LogVerbose(LogGroups.Console, string.Format("{0} Encoder: RPM {1} AvgPoll {2}", outputState.TimeStamp.ToString("HH:mm:ss.fffffff"), _state.CurrentMotorRpm, _state.AvgEncoderPollingRateMs, outputState.RegulationMode )); long newEncoderDegrees = outputState.EncoderCount * reversePolaritySign; long newResettableEncoderDegrees = outputState.ResettableCount * reversePolaritySign; bool encodersChanged = (_state.ResetableEncoderDegrees != newResettableEncoderDegrees); // Update the current encoder readings as soon as possible. _state.CurrentPower = ((double)(outputState.PowerSetPoint * reversePolaritySign)) / 100.0; _state.CurrentEncoderTimeStamp = outputState.TimeStamp; _state.CurrentEncoderDegrees = newEncoderDegrees; _state.ResetableEncoderDegrees = newResettableEncoderDegrees; if (encodersChanged) { // Prepare the encoder notification pxencoder.UpdateTickCount tickNotification = new pxencoder.UpdateTickCount( new pxencoder.UpdateTickCountRequest(outputState.TimeStamp, (int)newResettableEncoderDegrees)); SendNotification<pxencoder.UpdateTickCount>(_subMgrPort, tickNotification); } // get a snapshot of the current state. MotorState currentState = _state.Clone(); #region Simple PD Control if (currentState.TargetEncoderDegrees != 0) { double direction = Math.Sign(currentState.TargetPower); double pwrtarget = currentState.TargetPower * direction; double enctarget = currentState.TargetEncoderDegrees * direction; double encoder = outputState.EncoderCount * direction; double rpm = (double)currentState.CurrentMotorRpm * direction; double msUntilNext = currentState.AvgEncoderPollingRateMs; // negative remaining means we have passed the target. double remaining = enctarget - encoder; int goodEnough = (msUntilNext < 100) ? 5 : (msUntilNext < 300) ? 20 : 45; bool targetReached = (remaining <= goodEnough); LogVerbose(LogGroups.Console, string.Format("UpdateState: TargetPower {5} CurrentEncoder {0} {1} Target {2} done? {3} RPM {4} AvgMs {6}", currentState.CurrentEncoderDegrees, currentState.CurrentEncoderTimeStamp, currentState.TargetEncoderDegrees, targetReached, currentState.CurrentMotorRpm, currentState.TargetPower, currentState.AvgEncoderPollingRateMs)); // If a prior encoder target was active, signal that it is cancelled. if (_targetEncoderPending && targetReached) { lock (_targetEncoderReachedPort) { if (_targetEncoderPending) { LogVerbose(LogGroups.Console, "Signal target is complete!"); _targetEncoderReachedPort.Post(true); _targetEncoderPending = false; } } } // How soon do we want to start slowing down? // This is a factor of how fast and for how long the motor has been building up inertia. double forwardLooking = (pwrtarget < 0.5) ? ((pwrtarget * 150.0) + 20) : ((pwrtarget * 200.0) + 20.0); double degreesAtNextNotification = forwardLooking; if (rpm != 0.0 && msUntilNext != 0.0) // The distance in degrees we will travel at this rpm by the next encoder reading. degreesAtNextNotification = (rpm * 360.0) * msUntilNext / 60000.0; // We are not up-to-speed, wait a little longer before stopping if (rpm < pwrtarget) { if (rpm > 0) forwardLooking *= 0.7; if (rpm < 0) LogVerbose(LogGroups.Console, string.Format("Traveling in the wrong direction: Pwr {0} Enc {1} Tgt {2} RPM {3} Direction {4} FwdLook: {5} Remain: {6}", pwrtarget, encoder, enctarget, rpm, direction, forwardLooking, remaining)); else LogVerbose(LogGroups.Console, string.Format("Not up to speed: Pwr {0} Enc {1} Tgt {2} RPM {3} Direction {4} FwdLook: {5} Remain: {6}", pwrtarget, encoder, enctarget, rpm, direction, forwardLooking, remaining)); } else { LogVerbose(LogGroups.Console, string.Format("Targeting: Pwr {0} Enc {1} Tgt {2} RPM {3} Direction {4} FwdLook: {5} Remain: {6}", pwrtarget, encoder, enctarget, rpm, direction, forwardLooking, remaining)); } if (remaining < Math.Max(degreesAtNextNotification, forwardLooking) || remaining < forwardLooking || rpm <= 0) { LegoOutputMode outputMode = LegoOutputMode.MotorOn | LegoOutputMode.Regulated; LegoRegulationMode regulationMode = LegoRegulationMode.Individual; RunState powerAdjustment = RunState.Constant; long tachoLimit = outputState.EncoderLimit; int powerSetPoint = outputState.PowerSetPoint * (int)direction; if (Math.Abs(remaining) < 5 && rpm == 0) { // If we are completely stopped, // set the brake/coast and disengage active monitoring // in the notification handler _state.TargetEncoderDegrees = 0; powerSetPoint = 0; if (currentState.TargetStopState == MotorStopState.Brake) { outputMode |= LegoOutputMode.Brake; } else { regulationMode = LegoRegulationMode.Idle; powerAdjustment = RunState.Idle; } } else if ( remaining >= 0 && ( (remaining < 100 && powerSetPoint > 0 && rpm > 30) || (remaining < goodEnough && rpm < 10))) { powerSetPoint = 0; if (remaining < (pwrtarget / 2.0)) outputMode |= LegoOutputMode.Brake; } else if (forwardLooking < degreesAtNextNotification && (rpm >= pwrtarget) /* and up-to-speed */) { // we need to stop before the next encoder reading powerSetPoint = 0; outputMode |= LegoOutputMode.Brake; // The ms until we reach the point where forwardlooking rotations are remaining. double msUntilWeShouldStop = (degreesAtNextNotification - forwardLooking) / ((rpm * 360.0) / 60000.0); yield return Timeout(msUntilWeShouldStop); } else if (remaining < 0) { // Overshot, reverse direction powerSetPoint = CalculateMaxMotorPower((long)-remaining, -currentState.TargetPower, rpm, degreesAtNextNotification) / 2; } else { powerSetPoint = CalculateMaxMotorPower((long)remaining, currentState.TargetPower, rpm, degreesAtNextNotification); } if (degreesAtNextNotification < remaining) powerAdjustment = RunState.RampDown; if ((powerSetPoint != outputState.PowerSetPoint) || (outputState.Mode != outputMode) || (rpm < pwrtarget)) { LogVerbose(LogGroups.Console, string.Format("Target: {0} Encoder: {1} New Power: {2}", currentState.TargetEncoderDegrees, newEncoderDegrees, powerSetPoint)); #region Send an interim LEGO Motor command LegoSetOutputState setOutputState = new LegoSetOutputState(currentState.MotorPort, powerSetPoint, outputMode, regulationMode, 0, /* turn ratio */ powerAdjustment, tachoLimit); setOutputState.RequireResponse = (rpm == 0.0); LogVerbose(LogGroups.Console, setOutputState.ToString()); yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(setOutputState), delegate(LegoResponse ok) { if (ok.Success) LogVerbose(LogGroups.Console, "Moto Power was set"); else LogError(LogGroups.Console, "Moto Power Failed: " + ok.ErrorCode.ToString()); }, delegate(Fault fail) { LogError(LogGroups.Console, "Moto Power Failed: " + fail.ToString()); }); #endregion } } else { LogVerbose(LogGroups.Console, string.Format("Remaining: {0} Current Power: {1} Forward Looking: {2}", remaining, currentState.TargetPower, forwardLooking)); } } #endregion } yield break; }