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