Esempio n. 1
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;
        }
        public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
        {
            ValidateDriveConfiguration(false);
            _state.TimeStamp = DateTime.Now;

            PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();

            // Add a coordination header to our motor requests
            // so that advanced motor implementations can
            // coordinate the individual motor reqests.
            coord.ActuatorCoordination coordination = new coord.ActuatorCoordination();

            motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
            {
                TargetPower = setDrivePower.Body.LeftWheelPower
            });
            leftPower.ResponsePort = responsePort;
            leftPower.AddHeader(coordination);
            _leftMotorPort.Post(leftPower);

            motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
            {
                TargetPower = setDrivePower.Body.RightWheelPower
            });
            rightPower.ResponsePort = responsePort;
            rightPower.AddHeader(coordination);
            _rightMotorPort.Post(rightPower);

            // send notification to subscription manager
            Update update = new Update(_state);

            SendNotification <Update>(_subMgrPort, update);

            Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
                                                                                    delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
            {
                if (successList.Count == 2)
                {
                    setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
                }

                foreach (Fault fault in failureList)
                {
                    setDrivePower.ResponsePort.Post(fault);
                    break;
                }
            }));

            yield break;
        }
        public IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
        {
            ValidateDriveConfiguration(false);
            _state.TimeStamp = DateTime.Now;

            PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();

            // Add a coordination header to our motor requests
            // so that advanced motor implementations can
            // coordinate the individual motor reqests.
            coord.ActuatorCoordination coordination = new coord.ActuatorCoordination();

            motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower } );
            leftPower.ResponsePort = responsePort;
            leftPower.AddHeader(coordination);
            _leftMotorPort.Post(leftPower);

            motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.RightWheelPower } );
            rightPower.ResponsePort = responsePort;
            rightPower.AddHeader(coordination);
            _rightMotorPort.Post(rightPower);

            // send notification to subscription manager
            Update update = new Update(_state);
            SendNotification<Update>(_subMgrPort, update);

            Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
                delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
                {
                    if (successList.Count == 2)
                        setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());

                    foreach (Fault fault in failureList)
                    {
                        setDrivePower.ResponsePort.Post(fault);
                        break;
                    }
                }));

            yield break;
        }