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; }