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));
}
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;
}
public void DriveSetDrivePowerHandler(drive.SetDrivePower setdrivepower)
{
this.controllerDrivePort.Post(
new drive.SetDrivePower()
{
Body = setdrivepower.Body, ResponsePort = setdrivepower.ResponsePort
});
}
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> 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);
}
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;
}
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;
}