Add MKS-SERVO42D/57D servo motor CAN protocoll

[Johannes-Thiel]

Motivation

For the gripper in the feldfreund we want to use the MKS-SERVO42D/57D servo motor. To communicate with this motor we want to use the can bus. This is a first implementation of the communication.

Implementation

Progress

• The implementation is complete.
• Tested on hardware (or is not necessary).
• Documentation has been updated (or is not necessary).

[JensOgorek]

It looks good for me, but I also tried to do an AI review on this as well. It found some things.

Review: PR #192 – Add MKS-SERVO42D/57D servo motor CAN protocol

Summary

This PR adds a new MksServoMotor module for controlling MKS SERVO42D/57D closed-loop stepper motors via CAN bus. The CAN protocol layer (checksum calculation, byte packing, message handling) is clean and well-structured. The module follows established patterns (CAN subscription via enable_shared_from_this, REGISTER_MODULE_DEFAULTS, proper factory registration in module.cpp). Documentation and registration checklist are complete.

Main concerns are around the position property semantics and the precision zero correction logic.

---

MAJOR

1. position property doesn't reflect actual motor position

The position property is set to the commanded relative delta each time, not to an accumulated or actual position:

// rotate() sets position to the relative argument, not accumulated
this->properties.at("position")->number_value = degrees;

If you call motor.rotate(10, ...) then motor.rotate(20, ...), motor.position shows 20 — not 30. Similarly, grip() sets it to 5.0 and release() to -40.0. Since send_rotate sends command 0xF5 (relative pulses), the property should either accumulate (position += degrees) or be read back from the motor. As-is, motor.position will mislead users who expect it to reflect the motor's actual position.

2. Precision zero correction logic looks off

In step_precision_zero() / PZ_WAIT_ERROR, after the first relative rotation of 240°, the "correction" sends another relative rotation of 240 - abs(error_degrees):

int32_t abs_error = this->angle_error_value < 0 ? -this->angle_error_value : this->angle_error_value;
double error_degrees = (double)abs_error * 360.0 / ANGLE_ERROR_COUNTS_PER_TURN;
double corrected = this->pz_target_degrees - error_degrees;
this->send_rotate(corrected, this->pz_speed, this->pz_acc);

Since send_rotate uses command 0xF5 (relative position), this results in a total rotation of roughly 480 - error degrees, not a small correction of the first move. Two concerns:

• Taking abs(error) discards the sign, so the correction always reduces the second move regardless of whether the motor overshot or undershot the first move.
• The variable name corrected suggests an error correction of the first rotation, but the actual effect is a second large move. If this is intentional (the gripper needs ~480° total), the code would really benefit from a comment explaining that intent.

Worth double-checking: is 0xF5 relative or absolute in your MKS firmware version? If it's absolute, the logic makes perfect sense. If it's relative, this either works by coincidence (error is always small enough) or the total ~480° rotation is intentional and should be documented.

3. grip(), release(), precision_zero() are delta-arm gripper specific

The docs already note that precision_zero() is Feldfreund-specific, which is good. Consider also noting in the docs that grip() and release() are gripper-specific convenience methods (hardcoded to 5° and -40° at speed 3000), so users of MKS motors in other contexts know these aren't general-purpose.

---

CLEANUP

• Unused include: #include "../utils/uart.h" in mks_servo_motor.cpp — no echo() calls in this file.
• Magic number: millis_since(...) >= 1000 in PZ_WAIT_AFTER_ZERO — the other wait times have named members (pz_rotate_wait_ms, pz_correct_wait_ms), this one should too for consistency.
• homing_state values undocumented: The docs expose motor.homing_state as int but don't document what values mean. At minimum, document the "done" (9) and "failed" (10) values so users can write useful rules like when motor.homing_state == 9 then ....

[Johannes-Thiel]

The rotate command is a absolute position from the zero position. With this the precision_zero() is working correctly.

[falkoschindler]

Just a quick review:

Markdown:

• The line length should be limited at roughly 120 characters. There's one with >250.
• There should be one sentence per line.
• Please document units (e.g. speed commands/properties).
• Why does speed expect an int and not float? Maybe it becomes clear when seeing the unit (steps?). Or is it a motor limitation?

MksServoMotor::handle_can_msg:

• The early exit is a bit weird because in principle it would still make sense to continue handling a message without parameters. But data[0] would fail. So I think we should merge it into one condition, checking the count first:

  if (count == 6 && data[0] == 0x39) {
      ...
  }

Note that I didn't read the C code line by line. I'll trust your judgment, AI reviews and hardware testing. Let me know if this is ok or if I should read it more carefully.

[Johannes-Thiel]

@falkoschindler The can protocol of the MKS motor will store the command byte in data[0]. For this reason we only want to handle the message with the correct command byte.

[falkoschindler]

@Johannes-Thiel There are still some issues, mostly with documentation, but apparently also one or two bugs in precision_zero:

Missing units for speed and acceleration parameters

The run(), rotate(), and stop() methods document value ranges but not units:

• run(speed, direction, acc): speed is "0-4095", acc is "0-255" — no units specified.
• rotate(degrees, speed, acc): speed is "0-65535", acc is "0-255" — no units specified. Note: different speed scale than run().
• stop(acc): acceleration range and unit are not mentioned at all. The explanatory text below the method table only covers run() and rotate().

If these are raw protocol values with no direct physical mapping, that should be stated explicitly. If they do map to physical units (e.g. RPM, deg/s, deg/s^2), those should be documented.

grip(), release() and precision_zero() are domain-specific

grip() and release() are hardcoded shortcuts for the Feldfreund gripper (rotate(5, 3000, 255) and rotate(-40, 3000, 255) respectively). They don't belong in a general-purpose motor driver module and should be removed from the C++ implementation. Users can achieve the same behavior with Lizard routines:

let grip do
    motor.rotate(5, 3000, 255)
end

let release do
    motor.rotate(-40, 3000, 255)
end

precision_zero() has a similar problem. The core algorithm — rotate to a target angle, read the angle error via CAN, apply a correction, then set coordinate zero — is a generally useful homing pattern for MKS servos. However, the implementation hardcodes Feldfreund-specific values: target angle (240°), working current (1000 mA), start position (-110°), and various timing constants (2s, 1s, 1s). Either make it generic by accepting parameters (target angle, start position, speed, current, timeouts), or remove it entirely. Note that unlike grip()/release(), this cannot be moved to Lizard user code because it relies on internal CAN angle error reads (0x39) that aren't exposed to user-space.

set_holding_current valid range unclear

The docs say the argument is a "percentage" but don't specify valid values. The implementation does (pct / 10) - 1 clamped to 0-9, so effective valid inputs are 10, 20, ..., 100 (in steps of 10). Values in between are truncated. This should be documented.

precision_zero() failure path is incomplete

When precision_zero() fails (state PZ_FAILED), homing_state is set to PZ_FAILED but homing_active is never set back to false. On success (PZ_DONE), homing_active is correctly cleared. This means after a failure, homing_active stays true forever, which is a bug in mks_servo_motor.cpp:156-158.

Additionally, pz_state is set to PZ_FAILED but never reset to PZ_IDLE. Since step() calls step_precision_zero() whenever pz_state != PZ_IDLE, the state machine keeps running every cycle after failure, hitting the default no-op branch indefinitely.

[falkoschindler]

I took care of most of these issue. Now we can test if the following Lizard routine works for homing the Feldfreund gripper. If it does, we can remove all the code related to step_precision_zero:

int sleep_start_time
let sleep_1s do
  sleep_start_time = core.millis
  await core.millis > sleep_start_time + 1000
end
let precision_zero do
  motor.working_current = 1000
  motor.position(240)
  await sleep_1s  # wait for the motor to move
  await sleep_1s
  motor.read_position_error()
  await sleep_1s  # wait for the motor to respond
  motor.position(240 - motor.error)
  await sleep_1s  # move to the stalled position
  motor.zero()
  await sleep_1s  # wait for zero to be applied
  motor.position(-110)
end

Note that I renamed "rotate" to "position", "run" to "speed" (in analogy to other motors) and "angle error" to "position error".