private void ParseData(Device ParseFor, byte[] Payload)
{
JointTelemetry tel = Telemetry[ParseFor];
CANPacket canPack = (CANPacket)Payload[0];
switch (canPack)
{
case CANPacket.TELEMETRY:
int Voltage = BitConverter.ToInt16(Payload.Take(3).ToArray(), 1);
int Current = BitConverter.ToInt16(Payload, 3);
tel.Voltage = Voltage;
tel.Current = Current;
break;
case CANPacket.STATUS:
// Basically doesn't exist as of now
break;
case CANPacket.ENCODER_CNT:
int EncoderCount = BitConverter.ToInt32(Payload, 1);
tel.Encoder = EncoderCount;
break;
case CANPacket.ERROR_MSG:
int Error = BitConverter.ToInt16(Payload, 0) & 0xFF;
tel.ErrorCode = Error;
break;
case CANPacket.MODEL_NUM:
tel.ModelNumber = BitConverter.ToInt16(Payload, 0) & 0xFF;
break;
case CANPacket.SERVO:
tel.ServoPos = BitConverter.ToInt16(Payload, 0) & 0xFF;
break;
case CANPacket.LASER:
// Does not exist yet
break;
}
UpdateGUI?.Invoke();
Telemetry[ParseFor] = tel;
}
