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 static void ParseDiff2Cmd(Packet incomingPacket) { ArmPacket packet = new ArmPacket() { TargetDeviceID = Device.DIFFERENTIAL_2, PacketType = (CANPacket)incomingPacket.Data.Payload[0], Payload = incomingPacket.Data.Payload, Priority = false }; ArmController.Send(packet); }
private static void ParseShoulderCmd(Packet incomingPacket) { ArmPacket packet = new ArmPacket() { TargetDeviceID = Device.SHOULDER, PacketType = (CANPacket)incomingPacket.Data.Payload[0], Payload = incomingPacket.Data.Payload, Priority = false }; ArmController.Send(packet); }