private static void UpdateTelemetry(ArmPacket incomingPacket)
{
JointTelemetry tel = Telemetry[incomingPacket.TargetDeviceID];
switch (incomingPacket.PacketType)
{
case CANPacket.ENCODER_CNT:
tel.Encoder = UtilData.ToInt(incomingPacket.Payload);
ENCODER_CNT_FLG = false;
break;
case CANPacket.MODEL_NUM:
tel.ModelNumber = UtilData.ToInt(incomingPacket.Payload);
break;
case CANPacket.TELEMETRY:
// Break up into Voltage and Current
byte[] voltageData = incomingPacket.Payload.Take(2).ToArray();
byte[] currentData = incomingPacket.Payload.Skip(2).Take(2).ToArray();
tel.Current = UtilData.ToInt(currentData);
tel.Voltage = UtilData.ToInt(voltageData);
break;
case CANPacket.SERVO:
tel.ServoPos = UtilData.ToInt(incomingPacket.Payload);
break;
case CANPacket.ERROR_MSG:
tel.ErrorCode = incomingPacket.Payload[0];
break;
}
Telemetry[incomingPacket.TargetDeviceID] = tel;
}
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;
}
