public void SetCoordinatedMotors(UpdateMotorSpeed[] motors)
{
//LogInfo("TrackRoamerBrickPowerService:SetCoordinatedMotors()");
MotorSpeed motorSpeed = new MotorSpeed()
{
Timestamp = DateTime.Now
};
// Default null which is ignored by the controller.
motorSpeed.LeftSpeed = null;
motorSpeed.RightSpeed = null;
// Combine the motor commands
foreach (UpdateMotorSpeed ms in motors)
{
if (ms.Body.LeftSpeed != null && ms.Body.LeftSpeed >= -1.0 && ms.Body.LeftSpeed <= 1.0)
{
motorSpeed.LeftSpeed = ms.Body.LeftSpeed;
}
if (ms.Body.RightSpeed != null && ms.Body.RightSpeed >= -1.0 && ms.Body.RightSpeed <= 1.0)
{
motorSpeed.RightSpeed = ms.Body.RightSpeed;
}
}
// Send a singe command to the controller:
UpdateMotorSpeed combinedRequest = new UpdateMotorSpeed(motorSpeed);
_mainPort.Post(combinedRequest);
Activate(Arbiter.Choice(combinedRequest.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
// send responses back to the original motors
foreach (UpdateMotorSpeed ms in motors)
{
ms.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
},
delegate(Fault fault)
{
// send failure back to the original motors
foreach (UpdateMotorSpeed ms in motors)
{
ms.ResponsePort.Post(fault);
}
}));
}
public UpdateMotorSpeed(MotorSpeed motorSpeed)
{
this.Body = motorSpeed;
}
