public void DriveSetDriveSpeedHandler(drive.SetDriveSpeed setdrivespeed) { this.controllerDrivePort.Post( new drive.SetDriveSpeed() { Body = setdrivespeed.Body, ResponsePort = setdrivespeed.ResponsePort }); }
public IEnumerator <ITask> DriveSetDriveSpeedHandler(drive.SetDriveSpeed setdrivespeed) { if (!this.state.DriveState.IsEnabled) { setdrivespeed.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Drive not enabled!")); yield break; } this.state.DriveState.TimeStamp = DateTime.Now; short leftWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * setdrivespeed.Body.LeftWheelSpeed); short rightWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Right] * setdrivespeed.Body.RightWheelSpeed); byte[] cmdpacket = board.CreatePacket <short>(board.SetTravelVelocityString, leftWheelSpeed, rightWheelSpeed); 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))); setdrivespeed.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); setdrivespeed.ResponsePort.Post(f); yield break; } this.state.DriveState.LeftWheel.WheelSpeed = setdrivespeed.Body.LeftWheelSpeed; this.state.DriveState.RightWheel.WheelSpeed = setdrivespeed.Body.RightWheelSpeed; setdrivespeed.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public void DriveSetSpeedHandler(diffdrive.SetDriveSpeed setSpeed) { if (this.driveEntity == null) { throw new InvalidOperationException("Simulation entity not registered with service"); } if (!this.state.DriveState.IsEnabled) { setSpeed.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled."))); LogError("SetSpeed request to disabled drive."); return; } this.driveEntity.SetVelocity((float)setSpeed.Body.LeftWheelSpeed, (float)setSpeed.Body.RightWheelSpeed); this.UpdateStateFromSimulation(); setSpeed.ResponsePort.Post(DefaultUpdateResponseType.Instance); // send update notification for entire state this.subMgrPort.Post(new submgr.Submit(this.state.DriveState, DsspActions.UpdateRequest)); }
public IEnumerator <ITask> SetDriveSpeedHandler(drive.SetDriveSpeed setDriveSpeed) { LogError("Drive speed is not implemented"); yield break; }