/// <summary>
/// Read Serial Port for echo
/// </summary>
/// <param name="outBuff">The outbound message to match</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetEcho(byte[] outBuff)
{
byte[] inBuff = new byte[outMessageSize];
ScribblerResponse response = null;
int ixOutBuff = 0;
DateTime lastbytetime = DateTime.Now;
try
{
while (ixOutBuff < outMessageSize) // && Compare(DateTime.Now, lastbytetime) < ReadTimeOut)
{
byte[] temp = new byte[1];
_serialPort.Read(temp, 0, 1); //get 1 byte
if (temp[0] == outBuff[ixOutBuff])
{
inBuff[ixOutBuff] = temp[0];
ixOutBuff++;
lastbytetime = DateTime.Now;
}
else
{
Console.WriteLine("Echo missmatch");
break;
}
}
response = new ScribblerResponse();
response.Data = (byte[])inBuff.Clone();
#if DEBUG
Console.Write("Echo: ");
foreach (byte b in response.Data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
#endif
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
}
return(response);
}
//private IEnumerator<ITask> ConfigureSensorHandler(ConfigureSensor command)
//{
// if (command.Body == null)
// {
// command.ResponsePort.Post(new Fault());
// yield break;
// }
// if (command.Body.Sensor == null || command.Body.Configuration == null)
// {
// command.ResponsePort.Post(new Fault());
// yield break;
// }
// switch (command.Body.Sensor.ToUpper())
// {
// case "LEFT":
// _state.LightLeftConfig = command.Body.Configuration;
// break;
// case "CENTER":
// _state.LightCenterConfig = command.Body.Configuration;
// break;
// case "RIGHT":
// _state.LightRightConfig = command.Body.Configuration;
// break;
// }
// command.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// yield break;
//}
/// <summary>
/// Send a command to the Scribbler and wait for a response.
/// </summary>
/// <param name="ready"></param>
/// <param name="legoCommand"></param>
/// <returns></returns>
private IEnumerator <ITask> SendScribblerCommandHandler(SendScribblerCommand command)
{
// Send command to robot and wait for echo and response
ScribblerResponse validResponse = _scribblerCom.SendCommand(command.Body);
if (validResponse == null)
{
LogError(LogGroups.Console, "Send Scribbler Command null response");
command.ResponsePort.Post(new Fault());
}
//else if (validResponse.GetType() == typeof(LegoResponseException))
//{
// // Pull exception text from response
// string errorMessage = "LEGO command: " + command.Body.LegoCommandCode.ToString() + " response generated an error: " + ((LegoResponseException)validResponse).ErrorMessage;
// command.ResponsePort.Post(new SoapFaultContext(new InvalidOperationException(errorMessage)).SoapFault);
//}
else
{
// Check to see if we need to update state
// based on the response received from LEGO.
// PortSet<DefaultUpdateResponseType, Fault> responsePort = UpdateCurrentState(validResponse);
// if (responsePort != null)
// {
// yield return Arbiter.Choice(responsePort,
// delegate(DefaultUpdateResponseType response) { },
// delegate(Fault fault)
// {
// LogError(LogGroups.Console, "Failed to update LEGO NXT service state", fault);
// });
// }
// PostCommandProcessing(validResponse);
//reset timer
PollTimer.Enabled = false;
PollTimer.Enabled = true;
//Update our state with the scribbler's response
UpdateState(validResponse);
command.ResponsePort.Post(validResponse);
}
// Ready to process another command
Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
yield break;
}
/// <summary>
/// Read Serial Port for echo
/// </summary>
/// <param name="outBuff">The outbound message to match</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetEcho(byte[] outBuff)
{
byte[] inBuff = new byte[outMessageSize];
ScribblerResponse response = null;
int ixOutBuff = 0;
try
{
while (ixOutBuff < outMessageSize)
{
byte[] temp = new byte[1];
_serialPort.Read(temp, 0, 1); //get 1 byte
if (temp[0] == outBuff[ixOutBuff])
{
inBuff[ixOutBuff] = temp[0];
ixOutBuff++;
}
}
response = new ScribblerResponse();
response.Data = (byte[])inBuff.Clone();
//DEBUG
//Console.Write("Echo: ");
//foreach (byte b in response.Data)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
}
return(response);
}
/// <summary>
/// Update state after recieving return data from robot
/// </summary>
/// <param name="response"></param>
private void UpdateState(ScribblerResponse response)
{
//initialize notify list
List <string> notify = new List <string>();
switch ((ScribblerHelper.Commands)response.CommandType)
{
case ScribblerHelper.Commands.GET_STATE:
ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Data[0], response.Data[1]);
if (_state.Stall != parse_get_state.Stall)
{
notify.Add("STALL");
_state.Stall = parse_get_state.Stall;
}
if (_state.LineLeft != parse_get_state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_get_state.LineLeft;
}
if (_state.LineRight != parse_get_state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_get_state.LineRight;
}
_state.LEDLeft = parse_get_state.LedLeft;
_state.LEDCenter = parse_get_state.LedCenter;
_state.LEDRight = parse_get_state.LedRight;
break;
case ScribblerHelper.Commands.GET_OPEN_LEFT:
bool New_Get_Open_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRLeft != New_Get_Open_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_Open_Left;
}
break;
case ScribblerHelper.Commands.GET_OPEN_RIGHT:
bool New_Get_Open_Right = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRRight != New_Get_Open_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_Open_Right;
}
break;
case ScribblerHelper.Commands.GET_STALL:
bool New_Get_Stall = response.Data[0] == 1;
if (_state.Stall != New_Get_Stall)
{
notify.Add("STALL");
_state.Stall = New_Get_Stall;
}
break;
case ScribblerHelper.Commands.GET_LIGHT_LEFT:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
break;
case ScribblerHelper.Commands.GET_LIGHT_CENTER:
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
break;
case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_LINE_RIGHT:
bool New_Get_Line_Right = response.Data[0] == 1;
if (_state.LineRight != New_Get_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_Line_Right;
}
break;
case ScribblerHelper.Commands.GET_LINE_LEFT:
bool New_Get_Line_Left = response.Data[0] == 1;
if (_state.LineLeft != New_Get_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_Line_Left;
}
break;
case ScribblerHelper.Commands.GET_NAME:
Encoding.ASCII.GetChars(response.Data, 0, 8);
break;
case ScribblerHelper.Commands.GET_LIGHT_ALL:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_IR_ALL:
bool newirleft = response.Data[0] == 1; //NOTE: Not inverting logic here
bool newirright = response.Data[1] == 1;
if (_state.IRLeft != newirleft)
{
notify.Add("IRLEFT");
_state.IRLeft = newirleft;
}
if (_state.IRRight != newirright)
{
notify.Add("IRRIGHT");
_state.IRRight = newirright;
}
break;
case ScribblerHelper.Commands.GET_LINE_ALL:
bool newlineleft = response.Data[0] == 1;
bool newlineright = response.Data[1] == 1;
if (_state.LineLeft != newlineleft)
{
notify.Add("LINELEFT");
_state.LineLeft = newlineleft;
}
if (_state.LineRight != newlineright)
{
notify.Add("LINERIGHT");
_state.LineRight = newlineright;
}
break;
case ScribblerHelper.Commands.GET_ALL:
bool New_Get_All_IR_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
bool New_Get_All_IR_Right = response.Data[1] == 1;
if (_state.IRLeft != New_Get_All_IR_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_All_IR_Left;
}
if (_state.IRRight != New_Get_All_IR_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_All_IR_Right;
}
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[7], response.Data[6] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
bool New_Get_All_Line_Left = response.Data[8] == 1;
bool New_Get_All_Line_Right = response.Data[9] == 1;
if (_state.LineLeft != New_Get_All_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_All_Line_Left;
}
if (_state.LineRight != New_Get_All_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_All_Line_Right;
}
bool newstall = response.Data[10] == 1;
if (_state.Stall != newstall)
{
notify.Add("STALL");
_state.Stall = newstall;
}
break;
case ScribblerHelper.Commands.GET_ALL_BINARY:
case ScribblerHelper.Commands.SET_MOTORS_OFF:
case ScribblerHelper.Commands.SET_MOTORS:
case ScribblerHelper.Commands.SET_LED_LEFT_ON:
case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
case ScribblerHelper.Commands.SET_LED_CENTER_ON:
case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
case ScribblerHelper.Commands.SET_SPEAKER:
case ScribblerHelper.Commands.SET_SPEAKER_2:
case ScribblerHelper.Commands.SET_NAME:
case ScribblerHelper.Commands.SET_LED_ALL_ON:
case ScribblerHelper.Commands.SET_LED_ALL_OFF:
case ScribblerHelper.Commands.SET_LOUD:
case ScribblerHelper.Commands.SET_QUIET:
ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Data[0]);
if (_state.IRLeft != parse_all_binary.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = parse_all_binary.IRLeft; //NOTE: Not inverting logic here
}
if (_state.IRRight != parse_all_binary.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = parse_all_binary.IRRight;
}
if (_state.LineLeft != parse_all_binary.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_all_binary.LineLeft;
}
if (_state.LineRight != parse_all_binary.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_all_binary.LineRight;
}
if (_state.Stall != parse_all_binary.Stall)
{
notify.Add("STALL");
_state.Stall = parse_all_binary.Stall;
}
break;
default:
LogError(LogGroups.Console, "Update State command missmatch");
break;
}
// 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);
}
public ScribblerResponseMessage(ScribblerResponse b)
{
base.Body = b;
}
/// <summary>
/// Read Serial Port for a number of bytes and put into ScribblerResponse
/// </summary>
/// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetCommandResponse(int nBytes)
{
byte[] inBuff = new byte[nBytes];
ScribblerResponse response = null;
int read = 0;
try
{
while (read < nBytes)
{
int canread;
while ((canread = _serialPort.BytesToRead) == 0)
{
; //spin
}
int needtoread = nBytes - read;
if (canread > needtoread)
{
_serialPort.Read(inBuff, read, needtoread);
read += needtoread;
}
else
{
_serialPort.Read(inBuff, read, canread);
read += canread;
}
//DEBUG
//foreach (byte b in inBuff)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
}
response = new ScribblerResponse(nBytes - 1);
response.CommandType = inBuff[inBuff.Length - 1];
for (int i = 0; i < inBuff.Length - 1; i++)
{
response.Data[i] = inBuff[i];
}
//DEBUG
//Console.Write("Got: ");
//foreach (byte b in response.Data)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
}
return(response);
}
/// <summary>
/// DO NOT USE THIS COMMAND DIRECTLY.
/// In Scribbler.cs, post a message to _scribblerComPort
/// </summary>
/// <param name="cmd"></param>
/// <returns></returns>
internal ScribblerResponse SendCommand(ScribblerCommand cmd)
{
ScribblerResponse echo = null;
ScribblerResponse response = null;
byte[] buffer = new byte[outMessageSize];
if (buffer != null)
{
int ix = 0;
//buffer = cmd.ToByteArray();
buffer[ix++] = cmd.CommandType;
if (cmd.Data != null && cmd.Data.Length > 0)
{
foreach (byte b in cmd.Data)
{
buffer[ix++] = b;
}
}
//fill to standard size
while (ix < outMessageSize)
{
buffer[ix++] = 0;
}
//DEBUG
//Console.Write("\nSent: ");
//foreach (byte b in buffer)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
// When requesting a response, clear the inbound buffer
if (_serialPort.BytesToRead > 0)
{
_serialPort.DiscardInBuffer();
}
try
{
_serialPort.Write(buffer, 0, ix);
}
catch
{
Console.WriteLine("Serial Port Timeout. Turn on Scribbler.");
//throw new IOException();
}
echo = GetEcho(buffer);
response = GetCommandResponse(helper.ReturnSize((ScribblerHelper.Commands)cmd.CommandType));
}
return(response);
}
/// <summary>
/// Update state after recieving return data from robot
/// </summary>
/// <param name="response"></param>
private void UpdateState(ScribblerResponse response)
{
//initialize notify list
List<string> notify = new List<string>();
switch ((ScribblerHelper.Commands)response.CommandType)
{
case ScribblerHelper.Commands.GET_STATE:
ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Data[0], response.Data[1]);
if (_state.Stall != parse_get_state.Stall)
{
notify.Add("STALL");
_state.Stall = parse_get_state.Stall;
}
if (_state.LineLeft != parse_get_state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_get_state.LineLeft;
}
if (_state.LineRight != parse_get_state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_get_state.LineRight;
}
_state.LEDLeft = parse_get_state.LedLeft;
_state.LEDCenter = parse_get_state.LedCenter;
_state.LEDRight = parse_get_state.LedRight;
break;
case ScribblerHelper.Commands.GET_OPEN_LEFT:
bool New_Get_Open_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRLeft != New_Get_Open_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_Open_Left;
}
break;
case ScribblerHelper.Commands.GET_OPEN_RIGHT:
bool New_Get_Open_Right = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRRight != New_Get_Open_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_Open_Right;
}
break;
case ScribblerHelper.Commands.GET_STALL:
bool New_Get_Stall = response.Data[0] == 1;
if (_state.Stall != New_Get_Stall)
{
notify.Add("STALL");
_state.Stall = New_Get_Stall;
}
break;
case ScribblerHelper.Commands.GET_LIGHT_LEFT:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
break;
case ScribblerHelper.Commands.GET_LIGHT_CENTER:
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
break;
case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_LINE_RIGHT:
bool New_Get_Line_Right = response.Data[0] == 1;
if (_state.LineRight != New_Get_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_Line_Right;
}
break;
case ScribblerHelper.Commands.GET_LINE_LEFT:
bool New_Get_Line_Left = response.Data[0] == 1;
if (_state.LineLeft != New_Get_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_Line_Left;
}
break;
case ScribblerHelper.Commands.GET_NAME:
Encoding.ASCII.GetChars(response.Data, 0, 8);
break;
case ScribblerHelper.Commands.GET_LIGHT_ALL:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_IR_ALL:
bool newirleft = response.Data[0] == 1; //NOTE: Not inverting logic here
bool newirright = response.Data[1] == 1;
if (_state.IRLeft != newirleft)
{
notify.Add("IRLEFT");
_state.IRLeft = newirleft;
}
if (_state.IRRight != newirright)
{
notify.Add("IRRIGHT");
_state.IRRight = newirright;
}
break;
case ScribblerHelper.Commands.GET_LINE_ALL:
bool newlineleft = response.Data[0] == 1;
bool newlineright = response.Data[1] == 1;
if (_state.LineLeft != newlineleft)
{
notify.Add("LINELEFT");
_state.LineLeft = newlineleft;
}
if (_state.LineRight != newlineright)
{
notify.Add("LINERIGHT");
_state.LineRight = newlineright;
}
break;
case ScribblerHelper.Commands.GET_ALL:
bool New_Get_All_IR_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
bool New_Get_All_IR_Right = response.Data[1] == 1;
if (_state.IRLeft != New_Get_All_IR_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_All_IR_Left;
}
if (_state.IRRight != New_Get_All_IR_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_All_IR_Right;
}
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[7], response.Data[6] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
bool New_Get_All_Line_Left = response.Data[8] == 1;
bool New_Get_All_Line_Right = response.Data[9] == 1;
if (_state.LineLeft != New_Get_All_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_All_Line_Left;
}
if (_state.LineRight != New_Get_All_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_All_Line_Right;
}
bool newstall = response.Data[10] == 1;
if (_state.Stall != newstall)
{
notify.Add("STALL");
_state.Stall = newstall;
}
break;
case ScribblerHelper.Commands.GET_ALL_BINARY:
case ScribblerHelper.Commands.SET_MOTORS_OFF:
case ScribblerHelper.Commands.SET_MOTORS:
case ScribblerHelper.Commands.SET_LED_LEFT_ON:
case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
case ScribblerHelper.Commands.SET_LED_CENTER_ON:
case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
case ScribblerHelper.Commands.SET_SPEAKER:
case ScribblerHelper.Commands.SET_SPEAKER_2:
case ScribblerHelper.Commands.SET_NAME:
case ScribblerHelper.Commands.SET_LED_ALL_ON:
case ScribblerHelper.Commands.SET_LED_ALL_OFF:
case ScribblerHelper.Commands.SET_LOUD:
case ScribblerHelper.Commands.SET_QUIET:
ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Data[0]);
if (_state.IRLeft != parse_all_binary.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = parse_all_binary.IRLeft; //NOTE: Not inverting logic here
}
if (_state.IRRight != parse_all_binary.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = parse_all_binary.IRRight;
}
if (_state.LineLeft != parse_all_binary.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_all_binary.LineLeft;
}
if (_state.LineRight != parse_all_binary.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_all_binary.LineRight;
}
if (_state.Stall != parse_all_binary.Stall)
{
notify.Add("STALL");
_state.Stall = parse_all_binary.Stall;
}
break;
default:
LogError(LogGroups.Console, "Update State command missmatch");
break;
}
// 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);
}
/// <summary>
/// Read Serial Port for a number of bytes and put into ScribblerResponse
/// </summary>
/// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetCommandResponse(int nBytes)
{
byte[] inBuff = new byte[nBytes];
ScribblerResponse response = null;
int read = 0;
try
{
while (read < nBytes)
{
int canread;
while ((canread = _serialPort.BytesToRead) == 0) ; //spin
int needtoread = nBytes - read;
if (canread > needtoread)
{
_serialPort.Read(inBuff, read, needtoread);
read += needtoread;
}
else
{
_serialPort.Read(inBuff, read, canread);
read += canread;
}
//DEBUG
//foreach (byte b in inBuff)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
}
response = new ScribblerResponse(nBytes-1);
response.CommandType = inBuff[inBuff.Length-1];
for (int i = 0; i < inBuff.Length - 1; i++)
response.Data[i] = inBuff[i];
//DEBUG
//Console.Write("Got: ");
//foreach (byte b in response.Data)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
}
return response;
}
/// <summary>
/// Read Serial Port for echo
/// </summary>
/// <param name="outBuff">The outbound message to match</param>
/// <returns>ScribblerResponse</returns>
private ScribblerResponse GetEcho(byte[] outBuff)
{
byte[] inBuff = new byte[outMessageSize];
ScribblerResponse response = null;
int ixOutBuff = 0;
try
{
while (ixOutBuff < outMessageSize)
{
byte[] temp = new byte[1];
_serialPort.Read(temp, 0, 1); //get 1 byte
if (temp[0] == outBuff[ixOutBuff])
{
inBuff[ixOutBuff] = temp[0];
ixOutBuff++;
}
}
response = new ScribblerResponse();
response.Data = (byte[])inBuff.Clone();
//DEBUG
//Console.Write("Echo: ");
//foreach (byte b in response.Data)
//{
// if (b != 0)
// Console.Write(b + " ");
// else
// Console.Write("` ");
//}
//Console.Write("\n");
//DEBUG
}
catch (Exception ex)
{
Console.WriteLine("GetCommandResponse Exception: " + ex);
}
return response;
}
