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;
}