public static byte[] MensajeToArray(Mensaje m)
{
byte[] msg;
int siz = 3 + m.nDatos;
msg = new byte[siz];
msg[0] = m._cmd;
msg[1] = (byte)m._nDatos;
for (int i = 0; i < m._nDatos; i++)
msg[2 + i] = m.Datos[i];
msg[siz - 1] = m._chk;
return msg;
}
public static Mensaje ArrayToMensaje(byte[] msg)
{
Mensaje m = new Mensaje();
m.CMD = msg[0];
m.nDatos = msg[1];
m.CheckSum = msg[0];
m.CheckSum += msg[1];
int siz = 3 + m.nDatos;
for (int i = 0; i < m.nDatos; i++)
{
m.Datos[i] = msg[2 + i];
m.CheckSum += msg[2 + i];
}
m.CheckSum =(byte)( m.CheckSum ^ m.CMD);
return m;
}
private void serialPort1_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
Mensaje m1 = new Mensaje();
while (serialPort1.BytesToRead == 0) ;
m1.CMD = (byte)serialPort1.ReadByte();
while (serialPort1.BytesToRead == 0) ;
m1.nDatos = (byte)serialPort1.ReadByte();
for (int i = 0; i < m1.nDatos; i++)
{
while (serialPort1.BytesToRead == 0) ;
m1.Datos[i] = (byte)serialPort1.ReadByte();
}
while (serialPort1.BytesToRead == 0) ;
m1.CheckSum = (byte)serialPort1.ReadByte();
if (m1.CMD == 0xB1)
{
motor_ctrl1.UpdateVel(m1.Datos[0]);
motor_ctrl2.UpdateVel(m1.Datos[1]);
motor_ctrl3.UpdateVel(m1.Datos[2]);
}
}