/// <summary>
/// Handles incoming sensor messages
/// </summary>
private IEnumerator <ITask> SensorNotificationHandler(SensorNotification message)
{
//initialize notify list
List <string> notify = new List <string>();
//update state
switch (ScribblerHelper.SensorType((byte)message.Sensor))
{
case "IRLeft":
_state.IRLeft = (message.Status > 0);
notify.Add("IRLEFT");
break;
case "IRRight":
_state.IRRight = (message.Status > 0);
notify.Add("IRRIGHT");
break;
case "Stall":
_state.Stall = (message.Status > 0);
notify.Add("STALL");
break;
case "LineLeft":
_state.LineLeft = (message.Status > 0);
notify.Add("LINELEFT");
break;
case "LineRight":
_state.LineRight = (message.Status > 0);
notify.Add("LINERIGHT");
break;
case "LightLeft":
_state.LightLeft = message.Status;
notify.Add("LIGHTLEFT");
break;
case "LightRight":
_state.LightRight = message.Status;
notify.Add("LIGHTRIGHT");
break;
case "LightCenter":
_state.LightCenter = message.Status;
notify.Add("LIGHTCENTER");
break;
//only notify if there is a change
case "AllBinary":
ScribblerHelper.AllBinaryDecomp newState = new ScribblerHelper.AllBinaryDecomp(message.Status);
if (newState.IRLeft != _state.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = newState.IRLeft;
}
if (newState.IRRight != _state.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = newState.IRRight;
}
if (newState.Stall != _state.Stall)
{
notify.Add("STALL");
_state.Stall = newState.Stall;
}
if (newState.LineLeft != _state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = newState.LineLeft;
}
if (newState.LineRight != _state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = newState.LineRight;
}
break;
default:
LogError("Unrecognized sensor type");
//throw new ArgumentException("Sensor update error");
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);
yield break;
}
/// <summary>
/// Serial Port data event handler
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
if (e.EventType == SerialData.Chars)
{
try
{
while (serialPort.BytesToRead >= 2)
{
int messageLength = serialPort.ReadByte();
while ((messageLength > 5 || messageLength == 0) && serialPort.BytesToRead > 0)
{
messageLength = serialPort.ReadByte();
}
if (messageLength >= 2)
{
byte[] data = new byte[messageLength];
serialPort.Read(data, 0, messageLength);
//debug
foreach (byte b in data)
{
if (b != 0)
{
Console.Write(b + " ");
}
else
{
Console.Write("` ");
}
}
Console.Write("\n");
if (ScribblerHelper.IsSensorResponse(data[0]))
{
SensorNotification sensorMsg = new SensorNotification(data[0], data[1]);
ScribblerComInboundPort.Post(sensorMsg);
}
else
{
ScribblerCommand echo = new ScribblerCommand();
echo.Data = data;
ScribblerComInboundPort.Post(echo);
}
}
}
}
catch (ArgumentException ex)
{
if (ex.Message == "Offset and length were out of bounds for the array or count is greater than the number of elements from index to the end of the source collection.")
{
Exception invalidBaudRate = new IOException();
System.Diagnostics.Debug.WriteLine("Invalid Baud Rate");
ScribblerComInboundPort.Post(invalidBaudRate);
}
else
{
Console.WriteLine("Error reading from the serial port in ComBase(): {0}", ex.Message);
System.Diagnostics.Debug.WriteLine("Error reading from the serial port in ComBase(): {0}", ex.Message);
ScribblerComInboundPort.Post(ex);
}
}
catch (TimeoutException ex)
{
ScribblerComInboundPort.Post(ex);
// Ignore timeouts for now.
}
catch (IOException ex)
{
Console.WriteLine("Error reading from the serial port in CommBase(): {0}", ex.Message);
ScribblerComInboundPort.Post(ex);
}
catch (Exception ex)
{
Console.WriteLine("Error reading from the serial port in CommBase(): {0}", ex.Message);
ScribblerComInboundPort.Post(ex);
}
}
}
