/// <summary>
/// Handles incoming set motor messages
/// </summary>
private IEnumerator <ITask> SetMotorHandler(SetMotor message)
{
// Requests come too fast, so dump ones that come in too fast.
if (RequestPending > 0)
{
message.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
RequestPending++;
if (message.Body.Motor.ToUpper().Contains("LEFT"))
{
_state.MotorLeft = message.Body.Speed;
}
else if (message.Body.Motor.ToUpper().Contains("RIGHT"))
{
_state.MotorRight = message.Body.Speed;
}
else
{
LogError("Motor name not set properly");
}
_scribblerComm.SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft));
yield return(Arbiter.Receive <ScribblerCommand>(false, _scribblerComPort,
delegate(ScribblerCommand response)
{
//if (response.Data[0] != (byte)ScribblerHelper.Commands.SET_MOTORS)
// LogError("SetMotor picked up a wrong echo");
//initialize notify list
List <string> notify = new List <string>();
notify.Add("MOTORS");
// notify general subscribers
subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
// notify selective subscribers
submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
subMgrPort.Post(sub);
//reply to say we are done
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
}
));
yield break;
}
/// <summary>
/// Handles incoming set motor messages
/// </summary>
private IEnumerator<ITask> SetMotorHandler(SetMotor message)
{
if (message.Body == null)
{
message.ResponsePort.Post(new Fault());
yield break;
}
if (message.Body.Motor == null)
{
message.ResponsePort.Post(new Fault());
yield break;
}
// Requests come too fast, so dump ones that come in too fast.
if (RequestPending > 0 && message.Body.Speed != 100)
{
message.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
RequestPending++;
if (message.Body.Motor.ToUpper().Contains("LEFT"))
{
_state.MotorLeft = message.Body.Speed;
}
else if (message.Body.Motor.ToUpper().Contains("RIGHT"))
{
_state.MotorRight = message.Body.Speed;
}
else
{
LogError("Motor name not set properly");
}
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return Arbiter.Receive<ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
//if (response.Data[0] != (byte)ScribblerHelper.Commands.SET_MOTORS)
// LogError("SetMotor picked up a wrong echo");
//initialize notify list
List<string> notify = new List<string>();
notify.Add("MOTORS");
// notify general subscribers
subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
// notify selective subscribers
submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
subMgrPort.Post(sub);
//reply to say we are done
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
}
);
yield break;
}
