コード例 #1
0
        public void DriveSetPowerHandler(diffdrive.SetDrivePower setPower)
        {
            if (this.driveEntity == null)
            {
                throw new InvalidOperationException("Simulation entity not registered with service");
            }

            if (!this.state.DriveState.IsEnabled)
            {
                setPower.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
                LogError("SetPower request to disabled drive.");
                return;
            }

            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.");
                return;
            }

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

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

            // send update notification for entire state
            this.subMgrPort.Post(new submgr.Submit(this.state.DriveState, DsspActions.UpdateRequest));
        }
コード例 #2
0
ファイル: ScribblerDrive.cs プロジェクト: yingted/Myro
 public IEnumerator <ITask> AllStopHandler(drive.AllStop allStop)
 {
     drive.SetDrivePower zeroPower = new drive.SetDrivePower();
     zeroPower.Body         = new drive.SetDrivePowerRequest(0.0, 0.0);
     zeroPower.ResponsePort = allStop.ResponsePort;
     _mainPort.Post(zeroPower);
     yield break;
 }
コード例 #3
0
 public void DriveSetDrivePowerHandler(drive.SetDrivePower setdrivepower)
 {
     this.controllerDrivePort.Post(
         new drive.SetDrivePower()
     {
         Body = setdrivepower.Body, ResponsePort = setdrivepower.ResponsePort
     });
 }
コード例 #4
0
ファイル: ScribblerDrive.cs プロジェクト: yingted/Myro
        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;
        }
コード例 #5
0
        public IEnumerator <ITask> DriveSetDrivePowerHandler(drive.SetDrivePower setdrivepower)
        {
            if (!this.state.DriveState.IsEnabled)
            {
                setdrivepower.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason(
                                                    soap.FaultCodes.Receiver,
                                                    DsspFaultCodes.OperationFailed,
                                                    "Drive not enabled!"));
                yield break;
            }

            this.state.DriveState.TimeStamp = DateTime.Now;

            sbyte leftDrivePower = (sbyte)(board.HBridgeReverseMax +
                                           ((setdrivepower.Body.LeftWheelPower - GDDriveMin)
                                            * DrivePowerScale));

            sbyte rightDrivePower = (sbyte)(board.HBridgeReverseMax +
                                            ((setdrivepower.Body.RightWheelPower - GDDriveMin)
                                             * DrivePowerScale));

            byte[] cmdpacket = board.CreatePacket <sbyte>(board.SetTravelPowerString, leftDrivePower, rightDrivePower);

            serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
            sg.Timeout = this.state.DefaultResponsePause;
            sg.Data    = new serialcomservice.Packet(cmdpacket);

            var resultPort = this.serialCOMServicePort.SendAndGet(sg);

            yield return(resultPort.Choice());

            soap.Fault f = (soap.Fault)resultPort;
            if (f != null)
            {
                LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                setdrivepower.ResponsePort.Post(f);
                yield break;
            }

            if (this.HasFWError((serialcomservice.Packet)resultPort))
            {
                f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
                setdrivepower.ResponsePort.Post(f);
                yield break;
            }

            this.state.DriveState.LeftWheel.MotorState.CurrentPower  = setdrivepower.Body.LeftWheelPower;
            this.state.DriveState.RightWheel.MotorState.CurrentPower = setdrivepower.Body.RightWheelPower;
            setdrivepower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
コード例 #6
0
        public IEnumerator <ITask> SetDrivePowerHandler(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(2);

            Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
            leftPower.ResponsePort = responsePort;
            leftPower.AddHeader(coordination);
            _leftMotorPort.Post(leftPower);

            Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
            rightPower.ResponsePort = responsePort;
            rightPower.AddHeader(coordination);
            _rightMotorPort.Post(rightPower);

            // send notification to subscription manager
            drive.Update update = new drive.Update(_state);
            SendNotification <drive.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;
        }
コード例 #7
0
ファイル: ScribblerDrive.cs プロジェクト: SamLin95/cs3630
 public IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
 {
     drive.SetDrivePower zeroPower = new drive.SetDrivePower();
     zeroPower.Body = new drive.SetDrivePowerRequest(0.0, 0.0);
     zeroPower.ResponsePort = allStop.ResponsePort;
     _mainPort.Post(zeroPower);
     yield break;
 }