/// <summary> /// Handle sensor notifications from the brick /// </summary> /// <param name="update"></param> private void NotificationHandler(brick.LegoSensorUpdate update) { nxtcmd.LegoResponseGetInputValues inputValues = new nxtcmd.LegoResponseGetInputValues(update.Body.CommandData); if (inputValues != null && inputValues.Success) { bool firstNotification = (_state.TimeStamp == DateTime.MinValue); _state.TimeStamp = inputValues.TimeStamp; if (_state.Intensity != inputValues.ScaledValue || firstNotification) { _state.Intensity = inputValues.ScaledValue; SendNotification<SoundSensorUpdate>(_subMgrPort, _state); SendNotification<pxanalogsensor.Replace>(_genericSubMgrPort, SyncGenericState()); } } }
/// <summary> /// Receive Battery Notifications /// </summary> /// <param name="update"></param> private void NotificationHandler(brick.LegoSensorUpdate update) { nxtcmd.LegoResponseGetBatteryLevel batteryLevel = new nxtcmd.LegoResponseGetBatteryLevel(update.Body.CommandData); if (batteryLevel != null && batteryLevel.Success) { if (_state.MaxVoltage < batteryLevel.Voltage) { _state.TimeStamp = update.Body.TimeStamp; _state.MaxVoltage = batteryLevel.Voltage; SaveState(_state); } if (batteryLevel.Voltage != 0 && _state.MinVoltage > batteryLevel.Voltage) { _state.TimeStamp = update.Body.TimeStamp; _state.MinVoltage = batteryLevel.Voltage; SaveState(_state); } double percentBatteryPower = batteryLevel.Voltage / _state.MaxVoltage; if (_state.PercentBatteryPower != percentBatteryPower || _state.CurrentBatteryVoltage != batteryLevel.Voltage) { _state.TimeStamp = update.Body.TimeStamp; _state.CurrentBatteryVoltage = batteryLevel.Voltage; _state.PercentBatteryPower = percentBatteryPower; SendNotification<pxbattery.Replace>(_subMgrPort, _state.SyncGenericState(ref _genericState)); } if (_state.CurrentBatteryVoltage < _state.CriticalBatteryVoltage) { pxbattery.UpdateCriticalBattery criticalBattery = new pxbattery.UpdateCriticalBattery(); criticalBattery.PercentCriticalBattery = _state.PercentBatteryPower; SendNotification<pxbattery.SetCriticalLevel>(_subMgrPort, criticalBattery); } } }
/// <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; }
/// <summary> /// Handle sensor notifications from the brick /// </summary> /// <param name="update"></param> private void NotificationHandler(brick.LegoSensorUpdate update) { LegoResponseGetInputValues inputValues = new LegoResponseGetInputValues(update.Body.CommandData); if (inputValues != null && inputValues.Success) { bool firstTime = (_state.TimeStamp == DateTime.MinValue); _state.TimeStamp = inputValues.TimeStamp; if (_state.Intensity != inputValues.ScaledValue || firstTime) { _state.Intensity = inputValues.ScaledValue; // Send notifications to both the generic and native subscribers SendNotification<pxanalogsensor.Replace>(_genericSubMgrPort, SyncGenericState()); SendNotification<Replace>(_subMgrPort, _state); } } }
/// <summary> /// Handle periodic sensor readings from the brick /// </summary> /// <param name="update"></param> private void NotificationHandler(brick.LegoSensorUpdate update) { nxtcmd.I2CResponseSonarSensor inputValues = new nxtcmd.I2CResponseSonarSensor(update.Body.CommandData); if (inputValues.Success) { bool firstTime = (_state.TimeStamp == DateTime.MinValue); _state.TimeStamp = inputValues.TimeStamp; if (_state.Distance != inputValues.UltraSonicVariable || firstTime) { _state.Distance = inputValues.UltraSonicVariable; SendNotification<SonarSensorUpdate>(_subMgrPort, _state); SendNotification<pxanalogsensor.Replace>(_genericSubMgrPort, SyncGenericState()); } } }
private void NotificationHandler(brick.LegoSensorUpdate update) { nxtcmd.LegoResponseGetInputValues inputValues = new nxtcmd.LegoResponseGetInputValues(update.Body.CommandData); if (inputValues != null && inputValues.Success) { bool touch = (inputValues.ScaledValue == 1); if (_state.TouchSensorOn != touch || _state.TimeStamp == DateTime.MinValue) { _state.TouchSensorOn = touch; _state.TimeStamp = inputValues.TimeStamp; // Send Touch Sensor notification SendNotification<TouchSensorUpdate>(_subMgrPort, _state); // Send contact sensor notification SyncGenericContactState(); SendNotification<contactsensor.Update>(_genericSubMgrPort, _genericSensor); } } }
/// <summary> /// Handle periodic sensor readings from the pxbrick /// </summary> /// <param name="update"></param> private IEnumerator<ITask> NotificationHandler(brick.LegoSensorUpdate update) { DriveState currentState = _state.Clone(); LegoResponseGetOutputState outputState = new LegoResponseGetOutputState(update.Body.CommandData); if (outputState.Success && _state.RuntimeStatistics.LeftEncoderTimeStamp < outputState.TimeStamp) { LogVerbose(LogGroups.Console, outputState.ToString()); int leftReversePolaritySign = (currentState.LeftWheel.ReversePolarity) ? -1 : 1; int rightReversePolaritySign = (currentState.LeftWheel.ReversePolarity) ? -1 : 1; double direction, enctarget, encoder, remaining; _state.TimeStamp = outputState.TimeStamp; if (currentState.LeftWheel.MotorPort == outputState.MotorPort && currentState.RuntimeStatistics.LeftEncoderCurrent != (outputState.EncoderCount * leftReversePolaritySign)) { if (currentState.LeftWheel.ReversePolarity) { outputState.EncoderCount *= -1; outputState.BlockTachoCount *= -1; outputState.ResettableCount *= -1; } currentState.RuntimeStatistics.LeftEncoderCurrent = outputState.EncoderCount; _state.RuntimeStatistics.LeftEncoderCurrent = currentState.RuntimeStatistics.LeftEncoderCurrent; _state.RuntimeStatistics.LeftEncoderTimeStamp = currentState.RuntimeStatistics.LeftEncoderTimeStamp = outputState.TimeStamp; direction = Math.Sign(currentState.RuntimeStatistics.LeftPowerTarget); enctarget = currentState.RuntimeStatistics.LeftEncoderTarget * direction; encoder = outputState.EncoderCount * direction; // negative remaining means we have passed the target. remaining = enctarget - encoder; if (enctarget != 0) { if (remaining <= 5 && _targetEncoderPending[LEFT]) { _state.RuntimeStatistics.LeftPowerCurrent = 0.0; _state.RuntimeStatistics.LeftPowerTarget = 0.0; _targetEncoderReachedPort[LEFT].Post(true); _targetEncoderPending[LEFT] = false; if (_internalPendingDriveOperation == pxdrive.DriveRequestOperation.RotateDegrees) { _state.RuntimeStatistics.RightPowerCurrent = 0.0; _state.RuntimeStatistics.RightPowerTarget = 0.0; _targetEncoderReachedPort[RIGHT].Post(true); _targetEncoderPending[RIGHT] = false; } } } } if (currentState.RightWheel.MotorPort == outputState.MotorPort && currentState.RuntimeStatistics.RightEncoderCurrent != (outputState.EncoderCount * ((currentState.RightWheel.ReversePolarity) ? -1 : 1))) { if (currentState.RightWheel.ReversePolarity) { outputState.EncoderCount *= -1; outputState.BlockTachoCount *= -1; outputState.ResettableCount *= -1; } currentState.RuntimeStatistics.RightEncoderCurrent = outputState.EncoderCount; _state.RuntimeStatistics.RightEncoderCurrent = currentState.RuntimeStatistics.RightEncoderCurrent; _state.RuntimeStatistics.RightEncoderTimeStamp = currentState.RuntimeStatistics.RightEncoderTimeStamp = outputState.TimeStamp; direction = Math.Sign(currentState.RuntimeStatistics.RightPowerTarget); enctarget = currentState.RuntimeStatistics.RightEncoderTarget * direction; encoder = outputState.EncoderCount * direction; // negative remaining means we have passed the target. remaining = enctarget - encoder; if (enctarget != 0) { if (remaining <= 5 && _targetEncoderPending[RIGHT]) { _state.RuntimeStatistics.RightPowerCurrent = 0.0; _state.RuntimeStatistics.RightPowerTarget = 0.0; _targetEncoderReachedPort[RIGHT].Post(true); _targetEncoderPending[RIGHT] = false; if (_internalPendingDriveOperation == pxdrive.DriveRequestOperation.RotateDegrees) { _state.RuntimeStatistics.LeftPowerCurrent = 0.0; _state.RuntimeStatistics.LeftPowerTarget = 0.0; _targetEncoderReachedPort[LEFT].Post(true); _targetEncoderPending[LEFT] = false; } } } } // Send encoder notifications. SendNotification<DriveEncodersUpdate>(_subMgrPort, currentState.RuntimeStatistics); } yield break; }
/// <summary> /// Receive Buttons Notifications /// </summary> /// <param name="update"></param> private void NotificationHandler(brick.LegoSensorUpdate update) { // Receive Button notifications from the NXT Brick LegoResponseGetButtonState buttonsUpdate = new LegoResponseGetButtonState(update.Body.CommandData); if (buttonsUpdate != null && buttonsUpdate.Success) { if (_state.Buttons.PressedLeft != buttonsUpdate.PressedLeft || _state.Buttons.PressedEnter != buttonsUpdate.PressedEnter || _state.Buttons.PressedRight != buttonsUpdate.PressedRight || _state.Buttons.PressedCancel != buttonsUpdate.PressedCancel || _state.Buttons.TimeStamp == DateTime.MinValue) { _state.Buttons.TimeStamp = update.Body.TimeStamp; _state.Buttons.PressedLeft = buttonsUpdate.PressedLeft; _state.Buttons.PressedEnter = buttonsUpdate.PressedEnter; _state.Buttons.PressedRight = buttonsUpdate.PressedRight; _state.Buttons.PressedCancel = buttonsUpdate.PressedCancel; SendNotification<ButtonsUpdate>(_subMgrPort, _state.Buttons); } } }
/// <summary> /// Process Brick State /// </summary> /// <param name="brickState"></param> private IEnumerator<ITask> ProcessBrickState(brick.NxtBrickState brickState) { foreach (string key in brickState.Runtime.Devices.Keys) { brick.AttachRequest device = brickState.Runtime.Devices[key]; if (device.Registration.DeviceType != LegoDeviceType.AnalogSensor && device.Registration.DeviceType != LegoDeviceType.DigitalSensor) { continue; } PortSet<DsspDefaultLookup, DsspDefaultGet> lookupPort = ServiceForwarder<PortSet<DsspDefaultLookup, DsspDefaultGet>>(device.Registration.ServiceUri); DsspDefaultLookup lu = new DsspDefaultLookup(); lookupPort.Post(lu); yield return Arbiter.Choice(lu.ResponsePort, delegate(LookupResponse luResponse) { foreach(PartnerType pt in luResponse.PartnerList) { // See if this service supports the analog sensor contract if (pt.Contract == analog.Contract.Identifier) { // Check if we have already processed this one. if (_sensorList.ContainsKey(pt.Service)) break; string name = device.Registration.Name; string model = device.Registration.DeviceModel; int hardwareIdentifier = NxtCommon.HardwareIdentifier(device.Registration.Connection.Port); LogVerbose(LogGroups.Console, string.Format("Configuring {0}:{1} on {2} with analog service at {3}", model, name, hardwareIdentifier, pt.Service)); analog.AnalogSensorOperations sensorPort = ServiceForwarder<analog.AnalogSensorOperations>(pt.Service); Activate(Arbiter.Choice(sensorPort.Subscribe(_analogSensorNotificationsPort), delegate(SubscribeResponseType response) { // Keep track of the subscription manager response // so that we can unsubscribe later. _sensorList.Add(pt.Service, response); }, delegate(Fault fault) { LogError(LogGroups.Console, string.Format("Failure subscribing to {0} on port {1}.", model, hardwareIdentifier)); })); foreach (SensorConfiguration cfg in _state.SensorConfiguration) { if (cfg.DeviceModel != model) continue; SensorRange range = new SensorRange(hardwareIdentifier, model, name, cfg.RangeName); PortConfiguration portConfig = new PortConfiguration(hardwareIdentifier, range.ContactSensorName, cfg.SuccessRangeMin, cfg.SuccessRangeMax); portConfig.AnalogSensorServiceUri = pt.Service; if (portConfig != null) _state.RuntimeConfiguration.Add(range, portConfig); } break; } } }, delegate(Fault f) { }); } }
/// <summary> /// Wait until the LEGO is connected to the hardware before initializing this service. /// </summary> /// <param name="ready"></param> private void InitializationHandler(brick.ConnectToHardware ready) { SpawnIterator(FindSensors); }