コード例 #1
0
        public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
        {
            // For now, just dump requests that come in too fast.
            if (_requestPending > 0)
            {
                setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }

            //flip direction if nessisary
            double revPow = setMotorPower.Body.TargetPower;

            if (_state.ReversePolarity)
            {
                revPow = (-1) * revPow;
            }

            //update state
            _state.CurrentPower = revPow;

            //convert power
            int power = (int)Math.Round(revPow * _state.PowerScalingFactor);

            //send hardware specific motor data
            legoNXT.LegoSetOutputState motordata = new legoNXT.LegoSetOutputState();
            motordata.OutputPort      = _state.HardwareIdentifier - 1;
            motordata.PowerSetPoint   = power;
            motordata.Mode            = legoNXT.LegoOutputMode.PowerControl;
            motordata.RegulationMode  = legoNXT.LegoRegulationMode.MotorSpeed;
            motordata.TurnRatio       = 0;
            motordata.RunState        = legoNXT.LegoRunState.Running;
            motordata.RotationLimit   = 0; //run forever
            motordata.RequireResponse = false;
            _requestPending++;

            Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motordata),
                                    delegate(legoNXT.LegoResponse status)
            {
                _requestPending--;
            },
                                    delegate(Fault failure)
            {
                _requestPending--;
            }
                                    ));

            //reply to sender
            setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
コード例 #2
0
        public virtual IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            // For now, just dump requests that come in too fast.
            if (_requestPending > 0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            //flip direction if necessary
            double revPow = update.Body.LeftWheelPower;

            if (_state.LeftWheel.MotorState.ReversePolarity)
            {
                revPow = (-1) * revPow;
            }


            //send left motor command
            nxt.LegoSetOutputState motors = new nxt.LegoSetOutputState();
            motors.OutputPort      = _state.LeftWheel.MotorState.HardwareIdentifier - 1;
            motors.PowerSetPoint   = (int)(update.Body.LeftWheelPower * _state.LeftWheel.MotorState.PowerScalingFactor);
            motors.Mode            = nxt.LegoOutputMode.PowerControl;
            motors.RegulationMode  = nxt.LegoRegulationMode.MotorSpeed;
            motors.TurnRatio       = 0;
            motors.RunState        = nxt.LegoRunState.Running;
            motors.RotationLimit   = 0; //run forever
            motors.RequireResponse = false;
            _requestPending++;

            PortSet <nxt.LegoResponse, Fault> resp1Port = _legoPort.SendLegoCommand(motors);

/*            Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
 *              delegate(nxt.LegoResponse response)
 *              {
 *                  _requestPending--;
 *              },
 *              delegate(Fault fault)
 *              {
 *                  _requestPending--;
 *              }
 *          ));
 */
            //send right motor command
            motors                 = new nxt.LegoSetOutputState();
            motors.OutputPort      = _state.RightWheel.MotorState.HardwareIdentifier - 1;
            motors.PowerSetPoint   = (int)(update.Body.RightWheelPower * _state.RightWheel.MotorState.PowerScalingFactor);
            motors.Mode            = nxt.LegoOutputMode.PowerControl;
            motors.RegulationMode  = nxt.LegoRegulationMode.MotorSpeed;
            motors.TurnRatio       = 0;
            motors.RunState        = nxt.LegoRunState.Running;
            motors.RotationLimit   = 0; //run forever
            motors.RequireResponse = false;
            _requestPending++;
            //Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
            //    delegate(nxt.LegoResponse response)
            //    {
            //        _requestPending--;
            //    },
            //    delegate(Fault fault)
            //    {
            //        _requestPending--;
            //    }
            //));

            yield return(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
                                        delegate(nxt.LegoResponse response)
            {
                _requestPending--;
            },
                                        delegate(Fault fault)
            {
                _requestPending--;
            }
                                        ));

            yield return(Arbiter.Choice(resp1Port,
                                        delegate(nxt.LegoResponse response)
            {
                _requestPending--;
            },
                                        delegate(Fault fault)
            {
                _requestPending--;
            }
                                        ));

            LogInfo(update.Body.LeftWheelPower * 100 + ":" + update.Body.RightWheelPower * 100);
            _state.TimeStamp = DateTime.Now;

            // forward notification
            SendNotification(_subMgrPort, new drive.Update(_state));

            //reply to sender
            update.ResponsePort.Post(new DefaultUpdateResponseType());
            yield break;
        }