Пример #1
0
        public IEnumerator<ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
        {
            // Requests come too fast, so dump ones that come in too fast.
            if (RequestPending > 0)
            {
                setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }

            //flip direction if necessary
            double revPow = setMotorPower.Body.TargetPower;
            if (_state.ReversePolarity)
                revPow *= -1.0;

            //update state
            _state.CurrentPower = revPow;

            //convert to native units
            revPow *= 100;
            revPow += 100;
            int power = (int)Math.Round(revPow);

            //send hardware specific motor data
            brick.SetMotorBody motordata = new brick.SetMotorBody();
            motordata.Motor = _state.Name;
            motordata.Speed = power;

            RequestPending++;

            Activate(Arbiter.Choice(_scribblerPort.SetMotor(motordata),
                 delegate(DefaultUpdateResponseType status)
                 {
                     setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                     RequestPending--;
                 },
                 delegate(soap.Fault failure)
                 {
                     setMotorPower.ResponsePort.Post(failure);
                     RequestPending--;
                 }
            ));

            yield break;
        }
Пример #2
0
 public virtual IEnumerator<ITask> GetHandler(motor.Get get)
 {
     get.ResponsePort.Post(_state);
     yield break;
 }
Пример #3
0
 public virtual IEnumerator<ITask> ReplaceHandler(motor.Replace replace)
 {
     _state = replace.Body;
     replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
     yield break;
 }
Пример #4
0
        void StopMotorWithEncoderHandler(Port<encoder.UpdateTickCount> encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort)
        {
            int stopWheelAt;
            bool ignore;

            switch (side)
            {
                case "left":
                    stopWheelAt = stopLeftWheelAt;
                    ignore = !_leftEncoderTickEnabled;
                    break;

                default:
                case "right":
                    stopWheelAt = stopRightWheelAt;
                    ignore = !_rightEncoderTickEnabled;
                    break;
            }

            if (!ignore)
            {
            #if TRACEDEBUGTICKS
                Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + "    will stop wheel at=" + stopWheelAt);
            #endif // TRACEDEBUGTICKS

                if (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);
                    Activate(Arbiter.Choice(stop.ResponsePort,
                        delegate(DefaultUpdateResponseType resp)
                        {
            #if TRACEDEBUGTICKS
                            Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped by encoder !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
            #endif // TRACEDEBUGTICKS
                        },
                        delegate(Fault fault) { LogError(fault); }
                    ));

            #if TRACEDEBUGTICKS
                    Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending to completionPort: DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
            #endif // TRACEDEBUGTICKS
                    completionPort.Post(drive.DriveStage.Completed);
                }
            }
        }
        public virtual IEnumerator<ITask> GenericSetMotorPowerHandler(pxmotor.SetMotorPower genericMotorPower)
        {
            if (!ValidateConnection(_state, genericMotorPower.ResponsePort))
                yield break;

            SetMotorRotation setMotorRotation = new SetMotorRotation(new SetMotorRotationRequest(genericMotorPower.Body.TargetPower));
            _internalMotorRotationPort.Post(setMotorRotation);

            yield return Arbiter.Choice(setMotorRotation.ResponsePort,
                delegate(DefaultUpdateResponseType response)
                {
                    SendNotification<pxmotor.SetMotorPower>(_subMgrPort, genericMotorPower);
                    genericMotorPower.ResponsePort.Post(response);
                },
                delegate(Fault fault)
                {
                    genericMotorPower.ResponsePort.Post(fault);
                });

            yield break;
        }
 public virtual IEnumerator<ITask> GenericMotorReplaceHandler(pxmotor.Replace replace)
 {
     _state.CopyFromGenericState(replace.Body);
     replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
     yield break;
 }
 public virtual IEnumerator<ITask> GenericMotorGetHandler(pxmotor.Get get)
 {
     get.ResponsePort.Post(_state.ToGenericState(_genericState));
     yield break;
 }
Пример #8
0
        public virtual IEnumerator<ITask> SetMotorPowerHandler(pxmotor.SetMotorPower update)
        {
            if (_state.HardwareIdentifier == 0)
            {
                throw new InvalidOperationException("TrackRoamer Motor configuration missing");
            }

            powerbrick.MotorSpeed motorSpeed = new powerbrick.MotorSpeed();

            // Default -1 which is ignored by the controller.
            motorSpeed.LeftSpeed = null;
            motorSpeed.RightSpeed = null;

            double power = update.Body.TargetPower * _state.PowerScalingFactor / 100.0d;		// PowerScalingFactor is percentage, 0 to 100

            if (power != 0.0d && Math.Abs(power) < MinimumMotorPower)
            {
                //string msg = string.Format("Warning: Motor {0} - requested power {1} less than minimum {2}", _state.HardwareIdentifier, power, MinimumMotorPower);
                //Tracer.Trace(msg);
                //LogWarning(msg);

                power = (power < 0) ? -MinimumMotorPower : MinimumMotorPower;
            }

            //Tracer.Trace(string.Format("Motor {0} - setting power to {1}", _state.HardwareIdentifier, power));

            if (_state.HardwareIdentifier == 1)
            {
                motorSpeed.LeftSpeed = _state.ReversePolarity ? -power : power;
            }
            else if (_state.HardwareIdentifier == 2)
            {
                motorSpeed.RightSpeed = _state.ReversePolarity ? -power : power;
            }
            else
            {
                throw new ArgumentException("TrackRoamerMotorService : SetMotorPowerHandler() invalid HardwareIdentifier=" + _state.HardwareIdentifier);
            }

            _state.CurrentPower = power;

            coord.ActuatorCoordination coordination = update.GetHeader<coord.ActuatorCoordination>();
            if (coordination == null)
            {
                _powerbrickPort.UpdateMotorSpeed(motorSpeed);
            }
            else
            {
                // TODO: Remove this extra code when we allow headers
                //       to flow with a causality across services.

                // Pass on the coordination to the trackroamerbot
                powerbrick.UpdateMotorSpeed updateMotorSpeed = new powerbrick.UpdateMotorSpeed(motorSpeed);
                updateMotorSpeed.AddHeader(coordination);
                _powerbrickPort.Post(updateMotorSpeed);
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }