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 virtual IEnumerator <ITask> UpdateMotorSpeedHandler(UpdateMotorSpeed update)
{
//LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler()");
coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>();
if (coordination != null && coordination.Count > 1)
{
if (!_coordinationList.ContainsKey(coordination.RequestId))
{
_coordinationList.Add(coordination.RequestId, new Port <UpdateMotorSpeed>());
Activate(
Arbiter.MultipleItemReceive <UpdateMotorSpeed>(
false,
_coordinationList[coordination.RequestId],
coordination.Count,
SetCoordinatedMotors));
}
_coordinationList[coordination.RequestId].Post(update);
yield break;
}
bool changed = ((update.Body.LeftSpeed >= 0 && _state.MotorSpeed.LeftSpeed != update.Body.LeftSpeed) ||
(update.Body.RightSpeed >= 0 && _state.MotorSpeed.RightSpeed != update.Body.RightSpeed));
if (update.Body.LeftSpeed != null)
{
if (update.Body.LeftSpeed >= -1.0 && update.Body.LeftSpeed <= 1.0)
{
//LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - LeftSpeed=" + update.Body.LeftSpeed + " requested");
_state.MotorSpeed.LeftSpeed = update.Body.LeftSpeed;
}
else
{
LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid LeftSpeed=" + update.Body.LeftSpeed + " requested, must be between -1.0 and +1.0");
}
}
if (update.Body.RightSpeed != null)
{
if (update.Body.RightSpeed >= -1.0 && update.Body.RightSpeed <= 1.0)
{
//LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - RightSpeed=" + update.Body.RightSpeed + " requested");
_state.MotorSpeed.RightSpeed = update.Body.RightSpeed;
}
else
{
LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid RightSpeed=" + update.Body.RightSpeed + " requested, must be between -1.0 and +1.0");
}
}
// If we are connected, send the speed to the robot wheels controller:
if (ensureHardwareController())
{
double?leftSpeed = _state.MotorSpeed.LeftSpeed;
double?rightSpeed = _state.MotorSpeed.RightSpeed;
Tracer.Trace("IP: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - speed L: " + leftSpeed + " R: " + rightSpeed);
// it will take time for upper layers to react on whiskers. We want to have some protection here, to avoid damage.
// Note: while moving forward the speeds are negative.
// cannot move forward if whiskers are pressed; replace it with backwards movement at half speed though:
if (leftSpeed < 0 && _state.Whiskers.FrontWhiskerLeft.GetValueOrDefault())
{
Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - left whisker pressed, speed " + leftSpeed + " reversed");
leftSpeed = -leftSpeed / 2;
}
if (rightSpeed < 0 && _state.Whiskers.FrontWhiskerRight.GetValueOrDefault())
{
Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - right whisker pressed, speed " + rightSpeed + " reversed");
rightSpeed = -rightSpeed / 2;
}
_brickConnection.SetSpeed(leftSpeed, rightSpeed);
}
// Send Notifications to subscribers
if (changed)
{
_subMgrPort.Post(new submgr.Submit(_state.MotorSpeed, DsspActions.UpdateRequest));
}
_state.TimeStamp = _state.MotorSpeed.Timestamp = DateTime.Now;
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
