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)
{
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(Fault failure)
{
setDrivePower.ResponsePort.Post(failure);
RequestPending--;
}
));
yield break;
}
