Esempio n. 1
0
        public override void SetMotors(float leftPower, float rightPower)
        {
            if (!motorsOn)
            {
                EnableMotors();
            }

            drive.SetDrivePowerRequest drivePowerReq = new drive.SetDrivePowerRequest();
            drivePowerReq.LeftWheelPower  = leftPower;
            drivePowerReq.RightWheelPower = rightPower;
            drive.SetDrivePower setDrivePower = new drive.SetDrivePower(drivePowerReq);
            drivePort.Post(setDrivePower);

            bool done = false;

            Arbiter.Activate(DssEnvironment.TaskQueue,
                             Arbiter.Receive <DefaultUpdateResponseType>(false,
                                                                         setDrivePower.ResponsePort,
                                                                         delegate(DefaultUpdateResponseType state)
            {
                done = true;
            }
                                                                         ));

            while (!done)
            {
                ;
            }
        }
 public IEnumerator <ITask> AllStopHandler(drive.AllStop allStop)
 {
     drive.SetDrivePower zeroPower = new drive.SetDrivePower();
     zeroPower.Body         = new drive.SetDrivePowerRequest(0.0d, 0.0d);
     zeroPower.ResponsePort = allStop.ResponsePort;
     _mainPort.Post(zeroPower);
     yield break;
 }
Esempio n. 3
0
 public virtual IEnumerator <ITask> AllStopHandler(drive.AllStop update)
 {
     // Create a Drive request with both wheels stopped.
     // Call the SetDrivePowerHandler, but redirect the responses
     // to our response port.
     drive.SetDrivePower setDrivePower = new drive.SetDrivePower(new drive.SetDrivePowerRequest(), update.ResponsePort);
     SpawnIterator <drive.SetDrivePower>(setDrivePower, SetDrivePowerHandler);
     yield break;
 }
Esempio n. 4
0
        public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
        {
            if (setDrivePower.Body == null)
            {
                LogError("setDrivePower.Body == null");
                setDrivePower.ResponsePort.Post(new Fault());
                yield break;
            }

            double leftPower  = setDrivePower.Body.LeftWheelPower;
            double rightPower = setDrivePower.Body.RightWheelPower;

            // Dump requests that come too fast.
            // but let stop messages through
            if (RequestPending > 1 && leftPower != 0 && rightPower != 0)
            {
                setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                yield break;
            }
            RequestPending++;

            BoundsCheck(leftPower);
            BoundsCheck(rightPower);

            //update state
            _state.TimeStamp = DateTime.Now;
            _state.LeftWheel.MotorState.CurrentPower  = leftPower;
            _state.RightWheel.MotorState.CurrentPower = rightPower;

            //convert to native units
            int leftPowerScaled  = (int)Math.Round(leftPower * 100 + 100);
            int rightPowerScaled = (int)Math.Round(rightPower * 100 + 100);

            //send hardware specific motor data
            brick.SetMotorsBody motordata = new brick.SetMotorsBody(leftPowerScaled, rightPowerScaled);

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

            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> SetPowerHandler(diffdrive.SetDrivePower setPower)
        {
            if (_entity == null)
            {
                throw new InvalidOperationException("Simulation entity not registered with service");
            }

            if (!_state.IsEnabled)
            {
                setPower.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
                LogError("SetPower request to disabled drive.");
                yield break;
            }

            if ((setPower.Body.LeftWheelPower > 1.0f) || (setPower.Body.LeftWheelPower < -1.0f) ||
                (setPower.Body.RightWheelPower > 1.0f) || (setPower.Body.RightWheelPower < -1.0f))
            {
                // invalid drive power
                setPower.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
                LogError("Invalid Power parameter in SetPowerHandler.");
                yield break;
            }


            // Call simulation entity method for setting wheel torque
            _entity.SetMotorTorque(
                (float)(setPower.Body.LeftWheelPower),
                (float)(setPower.Body.RightWheelPower));

            UpdateStateFromSimulation();
            setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            // send update notification for entire state
            _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
            yield break;
        }
Esempio n. 7
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;
        }
        public IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
        {
            /*
                from http://social.msdn.microsoft.com/Forums/en-US/roboticssimulation/thread/13fb26c8-75fe-43b9-9e65-6a915e7d2560 :
                In RDS 2008 there is some discussion of the Generic Differential Drive in the Help file.
                AllStop should cause the current operation to be cancelled, as you expect.
                It will then disable the drive and you will have to issue an EnableDrive request before you can get the robot to move again.
                This is by design so that AllStop can act as an Emergency Stop and prevent any further action without an explicit reset.
                There should be a notification from the drive service that the drive power has been set to zero after the AllStop.
             */

            #if TRACELOG
            LogInfo("TrackRoamerDriveService:: AllStopHandler()");
            #endif

            cancelCurrentOperation();

            drive.SetDrivePower zeroPower = new drive.SetDrivePower();
            zeroPower.Body = new drive.SetDrivePowerRequest(0.0d, 0.0d);
            zeroPower.ResponsePort = allStop.ResponsePort;
            _mainPort.Post(zeroPower);

            pollEncoderState();     // get starting encoder state

            yield break;
        }
 public IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
 {
     drive.SetDrivePower zeroPower = new drive.SetDrivePower();
     zeroPower.Body = new drive.SetDrivePowerRequest(0.0d, 0.0d);
     zeroPower.ResponsePort = allStop.ResponsePort;
     _mainPort.Post(zeroPower);
     yield break;
 }