Beispiel #1
0
        /// <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;
        }