/*
* const char s_annc[] PROGMEM = "$01EFK%6lx\n";
* const char s_error[] PROGMEM = "$02%6lx%2x%3i\n"; //time, state, result
* // time,leftdistance,rightdistance,leftmotor,rightmotor,Direction,compass,front,left,right
* const char s_output[] PROGMEM = "$03%6lx6x%6x%c%3x%3xZ\n";
*
* const char s_AckWatchdog[] PROGMEM = "$04%6lx\n";
* const char s_AckMotor[] PROGMEM = "$05%6lx%4x\n";
* const char s_Calibrate[] PROGMEM = "$06%6lx%4i\n";
* $03 aed 0 0 0 0FZ
* *
*/
OutputArgs ParseSensor(byte[] Data)
{
int Index, maxlen = 29;
OutputArgs oArgs = new OutputArgs();
if (oArgs.HasCompass)
{
maxlen += 4;
}
if (oArgs.NumberSonar > 0)
{
maxlen += (oArgs.NumberSonar * 4);
}
if (Data.Length == maxlen)
{
if (Data[1] == 48 && Data[2] == 51)
{
oArgs.TimeStamp = ParseHex(Data, 3, 6);
oArgs.LeftTicks = (int)ParseHex(Data, 9, 6);
oArgs.RightTicks = (int)ParseHex(Data, 15, 6);
oArgs.LeftMotor = (ushort)ParseHex(Data, 21, 3);
oArgs.RightMotor = (ushort)ParseHex(Data, 24, 3);
oArgs.Direction = (char)Data[27];
Index = 28;
if (oArgs.HasCompass)
{
oArgs.Compass = (ushort)ParseHex(Data, Index, 4);
Index += 4;
}
if (oArgs.NumberSonar > 0)
{
oArgs.Front = (ushort)ParseHex(Data, Index, 4);
Index += 4;
if (oArgs.NumberSonar > 1)
{
if (oArgs.NumberSonar > 2)
{
oArgs.Left = (ushort)ParseHex(Data, Index, 4);
}
else
{
oArgs.Right = (ushort)ParseHex(Data, Index, 4);
}
Index += 4;
if (oArgs.NumberSonar > 2)
{
oArgs.Right = (ushort)ParseHex(Data, Index, 4);
}
}
}
}
}
return(oArgs);
}
private void ProcessIncomingData(List <byte> PortBuffer)
{
byte[] Data;
int Front, Back = 0;
if (PortBuffer.Count > 0)
{
Front = PortBuffer.IndexOf((byte)36);
if (Front > -1)
{
Back = PortBuffer.IndexOf((byte)10, Front);
if (Back > -1)
{
Data = new byte[Back - Front];
PortBuffer.CopyTo(Front, Data, 0, Back - Front);
Front = ((Data[1] - 48) * 10) + (Data[2] - 48);
switch (Front)
{
case (int)RobotOutputType.Announcement:
if (this.AnnounceReceived != null)
{
AnnounceArgs oArgs = new AnnounceArgs();
this.AnnounceReceived(this, oArgs);
}
break;
case (int)RobotOutputType.Error:
if (this.ErrorReceived != null)
{
ErrorArgs oArgs = ParseError(Data);
this.ErrorReceived(this, oArgs);
}
break;
case (int)RobotOutputType.Sensor:
if (this.OutputReceived != null)
{
OutputArgs oArgs = ParseSensor(Data);
this.OutputReceived(this, oArgs);
}
break;
case (int)RobotOutputType.Acknowledge:
if (this.ACKReceived != null)
{
ACKArgs oArgs = new ACKArgs();
oArgs.TimeStamp = ParseHex(Data, 3, 6);
this.ACKReceived(this, oArgs);
}
break;
case (int)RobotOutputType.Configuration:
if (this.ConfigReceived != null)
{
ConfigArgs oArgs = ParseConfig(Data);
this.ConfigReceived(this, oArgs);
}
break;
default:
break;
}
PortBuffer.RemoveRange(0, Back);
// Search through the bytes to find the data you want
// then remove the data from the list. It is good to
// clean up unused bytes (usually everything before
// what you're looking for)
}
// System.Windows.Forms.Message M = new System.Windows.Forms.Message("a");
// MessageReceivedCallBack
}
}
}