Codecs¶

codec.

MIT mode¶

MIT-mode codec. Pure: bytes in, bytes out.

Manual v1.0.18 pp.60-68. Command and reply are both standard frames with DLC 8.

Command bit layout:

D0 = p >> 8                      D1 = p & 0xFF
D2 = v >> 4                      D3 = (v & 0xF) << 4 | kp >> 8
D4 = kp & 0xFF                   D5 = kd >> 4
D6 = (kd & 0xF) << 4 | t >> 8    D7 = t & 0xFF

Reply:

D0        = driver id
D1..D2    = position, 16 bit
D3, D4hi  = velocity, 12 bit
D4lo, D5  = torque,   12 bit
D6        = temperature + 40
D7        = fault code

No field can encode an exact zero: each is a symmetric range over an even-sized field, so the midpoint sits half an LSB above zero. On an AK40-10 a commanded 0.0 N*m arrives as +1.22 mN*m. Far below the 60 mN*m needed to break the output away, but worth knowing before treating a zero command as an exact null.

Note that the special payloads below share the command encoding space: a saturated command can land exactly on one of them. pack_command() detects that and steps one torque LSB away, because the alternative is a silent mid-motion re-zero.

The third reply quantity is torque, not current - the manual’s own unpack_reply names it so and scales it by the torque field. TMotorCANControl converts it to a “q-axis current” through Kt, the gear ratio and an undocumented 0.59 factor, then presents that as the primary reading. We return the torque the wire carries; deriving a current from it is an explicitly-modelled step elsewhere.

TEMPERATURE_OFFSET¶: Reply byte 6 carries temperature + 40, giving a -40..215 C range.

class MitFeedback( motor_id: int, position_rad: float, velocity_radps: float, torque_nm: float, temperature_c: int, fault_code: int, )[source]¶

Bases: object

One decoded MIT reply, in the wire’s own terms.

pack_command( fields: MitFields, *, position_rad: float, velocity_radps: float, kp: float, kd: float, torque_nm: float, ) → bytes[source]¶

Quantise and pack the five command fields into 8 bytes.

Values outside a field’s range are clamped: the wire physically cannot express them. Clamping against the motor’s physical limits is a policy decision and happens a layer up, where it can be reported to the caller.

unpack_feedback( fields: MitFields, data: bytes, ) → MitFeedback[source]¶

Decode an 8-byte MIT reply.

Raises MalformedFrame and nothing else, for any input.

command_frame( fields: MitFields, motor_id: int, *, position_rad: float, velocity_radps: float, kp: float, kd: float, torque_nm: float, ) → Frame[source]¶: A MIT command as a standard frame addressed to motor_id.

enter_mit_frame(motor_id: int) → Frame[source]¶: Enter MIT control mode. Must be sent before any command is honoured.

exit_mit_frame(motor_id: int) → Frame[source]¶: Leave MIT control mode.

zero_position_frame(motor_id: int) → Frame[source]¶

Set the current position as zero.

The manual does not say whether this persists across a power cycle, so callers should not assume either way.

is_special(data: bytes) → bool[source]¶: True for the enter/exit/zero payloads, which are not commands.

Servo mode over CAN¶

Servo-mode CAN codec. Pure: bytes in, bytes out.

Manual v1.0.18 pp.35-45. Servo frames are extended, and the arbitration id carries the packet id above the motor id:

arbitration_id = (packet_id << 8) | motor_id

Three details here are each a bug in TMotorCANControl:

SET_POS scales degrees by 1e4, not 1e6 (a 100x error).
SET_POS_SPD divides speed and acceleration by 10 before packing them as int16.
Replies come in three flavours and only 0x29 is state. 0x2C is the “entered servo mode” handshake with a fixed FA FB FC FD payload and 0x09 is a bootloader jump; decoding either as position yields a plausible-looking lie.

Payload lengths differ per packet - 4 bytes for the scalar setpoints, 1 for origin, 8 for position-velocity - so DLC is not a constant.

class ServoPacket(*values)[source]¶

Bases: IntEnum

Command packet ids, from the manual’s CAN_PACKET_ID enum.

SET_MIT¶: Listed in the manual’s enum with no payload or example. Deliberately has no encoder: a guessed payload on a packet id that exists is worse than none.

class ServoFunction(*values)[source]¶

Bases: IntEnum

Function ids the driver replies with.

class OriginMode(*values)[source]¶

Bases: IntEnum

Argument to SET_ORIGIN.

TEMPORARY¶: Cleared on power loss. The safe default.

PERMANENT¶: Writes flash. The manual restricts this to dual-encoder models.

class ServoFeedback( motor_id: int, position_deg: float, velocity_erpm: float, current_a: float, temperature_c: int, fault_code: int, )[source]¶

Bases: object

A decoded 0x29 status frame, in the wire’s own units.

class ServoEventFrame(kind: str, function_id: int, payload: bytes)[source]¶

Bases: object

A reply that is not state. The motor layer timestamps it.

arbitration_id( packet: ServoPacket | int, motor_id: int, ) → int[source]¶

The extended arbitration id for a servo packet: (packet_id << 8) | motor_id.

Servo mode puts the command above the motor id in one 29-bit extended id, which is why servo framing is unambiguous where MIT’s is not.

split_arbitration_id(arb: int) → tuple[int, int][source]¶: Return (function_or_packet_id, motor_id).

encode_duty( motor_id: int, duty: float, scaling: ServoScaling = SERVO_CAN_COMMON, ) → Frame[source]¶: Duty-cycle mode. duty is -1.0..1.0.

encode_current( motor_id: int, amps: float, scaling: ServoScaling = SERVO_CAN_COMMON, ) → Frame[source]¶: Current-loop mode, i.e. torque control. amps is -60..60.

encode_current_brake( motor_id: int, amps: float, scaling: ServoScaling = SERVO_CAN_COMMON, ) → Frame[source]¶: Current-brake mode. Holds position with a braking current; 0..60 A, never negative.

encode_rpm( motor_id: int, erpm: float, scaling: ServoScaling = SERVO_CAN_COMMON, ) → Frame[source]¶: Velocity mode. erpm is electrical RPM, -100000..100000.

encode_position( motor_id: int, degrees: float, scaling: ServoScaling = SERVO_CAN_COMMON, ) → Frame[source]¶: Position mode. Degrees are scaled by 1e4 - the reference library uses 1e6.

encode_origin( motor_id: int, mode: OriginMode = OriginMode.TEMPORARY, ) → Frame[source]¶

Set the current position as origin. One payload byte.

This codec does not police OriginMode.PERMANENT; the capability check lives on the spec, where it can name the motor and refuse before a frame is built.

encode_position_speed( motor_id: int, degrees: float, speed_erpm: float, accel_erpm_s2: float, scaling: ServoScaling = SERVO_CAN_COMMON, ) → Frame[source]¶

Position-velocity mode: a trapezoidal move to degrees.

Speed and acceleration are packed as int16 after dividing by 10, so one speed count is 10 ERPM and one acceleration count is 10 ERPM/s^2. The reference library omits both divisors. Acceleration is unsigned per the manual.

classify( frame: Frame, ) → ServoFunction | None[source]¶: Identify a reply, or None if it is not one we recognise.

decode_status( data: bytes, scaling: ServoScaling = SERVO_CAN_COMMON, ) → ServoFeedback[source]¶

Decode a 0x29 status payload.

Uses explicit two’s-complement arithmetic rather than NumPy. np.int16 raises OverflowError on NumPy 2 for any value with the high bit set, which is what makes the reference library’s receive path die on every negative position.

decode( frame: Frame, scaling: ServoScaling = SERVO_CAN_COMMON, ) → ServoFeedback | ServoEventFrame[source]¶

Decode any recognised servo reply.

Returns ServoFeedback only for 0x29. The handshake and bootloader frames come back as ServoEventFrame so they can never be mistaken for a position.