public OutMessage(byte[] values)
{
if (values.Length < 4 || values.Length > 5)
{
throw new Exception("Wrong input.");
}
message = new byte[5];
for (int i = 0; i < 4; i++)
{
message[i] = values[i];
}
if (values.Length == 5)
{
message[4] = values[4];
if (CRC8.Checksum(message) != 0)
{
throw new Exception("Wrong CRC.");
}
}
else
{
message[4] = (byte)(CRC8.Checksum(message) & 0xff);
}
}
public OutMessage(byte seq, byte tacho, byte speed, byte brake)
{
message = new byte[5];
message[0] = seq;
message[1] = tacho;
message[2] = speed;
message[3] = brake;
message[4] = (byte)(CRC8.Checksum(message) & 0xff);
}
public override string ToString()
{
string toReturn = string.Format("{0, -5} {1, -4} ", message[0], message[1]);
for (int i = 2; i < 14; i += 2)
{
toReturn += string.Format("{0,-10} ", CRC8.ByteToShort(message[i + 1], message[i]));
}
toReturn += string.Format("{0, -5}", message[14]);
return(toReturn);
}
public InMessage(byte seq, byte tacho, short XAcceleration,
short YAcceleration, short ZAcceleration, short XRotation,
short YRotation, short ZRotation, byte crc)
{
message = new byte[15];
message[0] = seq;
message[1] = tacho;
message[2] = CRC8.ShortToByte(XAcceleration)[0];
message[3] = CRC8.ShortToByte(XAcceleration)[1];
message[4] = CRC8.ShortToByte(YAcceleration)[0];
message[5] = CRC8.ShortToByte(YAcceleration)[1];
message[6] = CRC8.ShortToByte(ZAcceleration)[0];
message[7] = CRC8.ShortToByte(ZAcceleration)[1];
message[8] = CRC8.ShortToByte(XRotation)[0];
message[9] = CRC8.ShortToByte(XRotation)[1];
message[10] = CRC8.ShortToByte(XRotation)[0];
message[11] = CRC8.ShortToByte(XRotation)[1];
message[12] = CRC8.ShortToByte(XRotation)[0];
message[13] = CRC8.ShortToByte(XRotation)[1];
message[14] = (byte)(CRC8.Checksum(message) & 0xff);
}
private void Read()
{
byte[] receiveBuffer;
while (true)
{
try
{
// ACK received
if (waitACK)
{
receiveBuffer = new byte[1];
if (port.BytesToRead >= 1)
{
port.Read(receiveBuffer, 0, 1);
Check(receiveBuffer[0]);
}
}
// Message received
else
{
receiveBuffer = new byte[15];
if (port.BytesToRead >= 15)
{
port.Read(receiveBuffer, 0, 15);
try
{
// Check CRC (if incorrect, exception will be thrown)
InMessage inMessage = new InMessage(receiveBuffer);
// Return ACK
Write(receiveBuffer[0]);
// If record received
if (receiveBuffer[0] == 0 || receiveBuffer[0] == 1)
{
// If message not previously received
if (messageReceived == null ||
inMessage.GetMessage()[0] != messageReceived.GetMessage()[0])
{
byte[] msg = inMessage.GetMessage();
int tacho = messageReceived == null ? msg[1] :
msg[1] + lap.Records[lap.Records.Count - 1].tacho;
short XAcc = CRC8.ByteToShort(msg[3], msg[2]);
short YAcc = CRC8.ByteToShort(msg[5], msg[4]);
short ZAcc = CRC8.ByteToShort(msg[7], msg[6]);
short XRot = CRC8.ByteToShort(msg[9], msg[8]);
short YRot = CRC8.ByteToShort(msg[11], msg[10]);
short ZRot = CRC8.ByteToShort(msg[13], msg[12]);
Record record = new Record(tacho, XAcc, YAcc, ZAcc, XRot, YRot, ZRot);
messageReceived = inMessage;
lap.AddRecord(record);
var form = Form.ActiveForm as Form1;
form.Invoke(new MethodInvoker(delegate
{
form.AddPoint(record);
//form.Write(inMessage.ToString());
//form.Write(record.ToString());
}));
}
}
else
{
Console("All messages received");
new Thread(SendCommands).Start();
}
}
catch (Exception) { }
}
}
}
catch (Exception)
{
Environment.Exit(-1);
}
}
}
