Add MKS-SERVO42D/57D servo motor CAN protocol

docs/module_reference.md

@@ -630,6 +630,49 @@ The optional acceleration argument defaults to 0, which starts and stops pulsing
 
 When the motor is disabled, it will stop and ignore movement commands.
 
+## MKS Servo Motor
+
+The MKS Servo Motor module controls an [MKS SERVO42D/57D](https://github.com/makerbase-mks) closed-loop stepper motor via CAN.
+
+| Constructor                                  | Description              | Arguments                |
+| -------------------------------------------- | ------------------------ | ------------------------ |
+| `motor = MksServoMotor(can, can_id)` | CAN module and CAN node ID | CAN module, `int` |
+
+| Properties              | Description                                 | Data type |
+| ----------------------- | ------------------------------------------- | --------- |
+| `motor.position`        | Motor position (degrees)                    | `float`   |
+| `motor.speed`           | Motor speed                                 | `int`     |
+| `motor.working_current` | Working current (mA, 0-3000, default: 1700) | `int`     |
+| `motor.enabled`         | Whether the motor is enabled                | `bool`    |
+| `motor.homing_state`    | Current state of the precision zero sequence | `int`    |
+| `motor.homing_active`   | Whether precision zero is in progress       | `bool`    |
+
+| Methods                                    | Description                                     | Arguments                     |
+| ------------------------------------------ | ----------------------------------------------- | ----------------------------- |
+| `motor.enable()`                           | Enable the motor                                |                               |
+| `motor.disable()`                          | Disable the motor                               |                               |
+| `motor.set_vfoc()`                         | Set VFOC mode                                   |                               |
+| `motor.zero()`                             | Set current position as zero                    |                               |
+| `motor.precision_zero()`                   | Start precision zeroing sequence                |                               |
+| `motor.set_working_current(ma)`            | Set working current (mA, 0-3000)                | `int`                         |
+| `motor.set_holding_current(pct)`           | Set holding current (percentage)                | `int`                         |
+| `motor.run(speed, direction, acc)`         | Run motor with speed, direction and acceleration | `int`, `int`, `int`          |
+| `motor.stop(acc)`                          | Stop motor with given deceleration              | `int`                         |
+| `motor.rotate(degrees, speed, acc)`        | Rotate by given degrees                         | `float`, `int`, `int`        |
+| `motor.grip()`                             | Grip (rotate 5 degrees at high speed)           |                               |
+| `motor.release()`                          | Release (rotate -40 degrees at high speed)      |                               |
+
+The `run()` method accepts a speed (0-4095), direction (0 or 1) and acceleration (0-255).
+
+The `rotate()` method moves the motor by a relative number of degrees with a given speed (0-65535) and acceleration (0-255).
+
+The `precision_zero()` method performs a multi-step zeroing sequence specific to the Feldfreund gripper: it rotates to a target angle, reads the angle error from the motor, applies a correction, sets the coordinate zero, and then moves to a start position.
+
+The `homing_state` property can be used in rules to react to the result of this sequence. Useful terminal values are:
+
+- `9`: done (`PZ_DONE`)
+- `10`: failed (`PZ_FAILED`)
+
 ## Motor Axis
 
 The motor axis module wraps a motor and two limit switches.

main/modules/mks_servo_motor.cpp

@@ -0,0 +1,270 @@
+#include "mks_servo_motor.h"
+#include <algorithm>
+
+REGISTER_MODULE_DEFAULTS(MksServoMotor)
+
+const std::map<std::string, Variable_ptr> MksServoMotor::get_defaults() {
+    return {
+        {"position", std::make_shared<NumberVariable>()},
+        {"speed", std::make_shared<IntegerVariable>()},
+        {"working_current", std::make_shared<IntegerVariable>(1700)},
+        {"enabled", std::make_shared<BooleanVariable>(false)},
+        {"homing_state", std::make_shared<IntegerVariable>(0)},
+        {"homing_active", std::make_shared<BooleanVariable>(false)},
+    };
+}
+
+MksServoMotor::MksServoMotor(const std::string name, const Can_ptr can, const uint16_t can_id)
+    : Module(mks_servo_motor, name), can(can), can_id(can_id) {
+    this->properties = MksServoMotor::get_defaults();
+    this->send_working_current(1700);
+}
+
+void MksServoMotor::subscribe_to_can() {
+    this->can->subscribe(this->can_id, std::static_pointer_cast<Module>(this->shared_from_this()));
+}
+
+void MksServoMotor::send(const uint8_t *data, uint8_t len) {
+    uint8_t buf[8] = {0};
+    uint8_t checksum = (uint8_t)(this->can_id & 0xFF);
+    for (uint8_t i = 0; i < len; i++) {
+        buf[i] = data[i];
+        checksum += data[i];
+    }
+    buf[len] = checksum & 0xFF;
+    this->can->send(this->can_id, buf, false, len + 1);
+}
+
+void MksServoMotor::send_enable(bool enable) {
+    uint8_t data[] = {0xF3, (uint8_t)(enable ? 0x01 : 0x00)};
+    this->send(data, 2);
+    this->properties.at("enabled")->boolean_value = enable;
+}
+
+void MksServoMotor::send_set_vfoc() {
+    uint8_t data[] = {0x82, 0x05};
+    this->send(data, 2);
+}
+
+void MksServoMotor::send_working_current(int64_t ma) {
+    ma = std::clamp(ma, (int64_t)0, MAX_WORKING_CURRENT_MA);
+    uint16_t val = (uint16_t)ma;
+    uint8_t data[] = {0x83, (uint8_t)(val >> 8), (uint8_t)(val & 0xFF)};
+    this->send(data, 3);
+    this->properties.at("working_current")->integer_value = ma;
+}
+
+void MksServoMotor::send_holding_current(int64_t pct) {
+    int64_t ratio = std::clamp((pct / 10) - 1, (int64_t)0, MAX_HOLDING_RATIO);
+    uint8_t data[] = {0x9B, (uint8_t)ratio};
+    this->send(data, 2);
+}
+
+void MksServoMotor::send_run_internal(int64_t direction, int64_t speed, int64_t acc) {
+    speed = std::clamp(speed, (int64_t)0, MAX_RUN_SPEED);
+    direction = std::clamp(direction, (int64_t)0, (int64_t)1);
+    acc = std::clamp(acc, (int64_t)0, MAX_ACC);
+    uint8_t byte1 = (uint8_t)((direction << 7) | ((speed >> 8) & 0x0F));
+    uint8_t byte2 = (uint8_t)(speed & 0xFF);
+    uint8_t data[] = {0xF6, byte1, byte2, (uint8_t)acc};
+    this->send(data, 4);
+}
+
+void MksServoMotor::send_stop_internal(int64_t acc) {
+    acc = std::clamp(acc, (int64_t)0, MAX_ACC);
+    uint8_t data[] = {0xF6, 0x00, 0x00, (uint8_t)acc};
+    this->send(data, 4);
+}
+
+void MksServoMotor::send_rotate_counts(int32_t counts, int64_t speed, int64_t acc) {
+    speed = std::clamp(speed, (int64_t)0, MAX_ROTATE_SPEED);
+    acc = std::clamp(acc, (int64_t)0, MAX_ACC);
+    counts = std::clamp(counts, INT24_MIN, INT24_MAX);
+    uint32_t raw = (uint32_t)counts;
+    uint8_t p2 = (raw >> 16) & 0xFF;
+    uint8_t p1 = (raw >> 8) & 0xFF;
+    uint8_t p0 = raw & 0xFF;
+    uint8_t data[] = {
+        0xF5,
+        (uint8_t)((speed >> 8) & 0xFF),
+        (uint8_t)(speed & 0xFF),
+        (uint8_t)acc,
+        p2,
+        p1,
+        p0,
+    };
+    this->send(data, 7);
+}
+
+void MksServoMotor::send_rotate(double degrees, int64_t speed, int64_t acc) {
+    int32_t counts = (int32_t)(degrees * COUNTS_PER_DEG);
+    this->send_rotate_counts(counts, speed, acc);
+}
+
+void MksServoMotor::send_coord_zero() {
+    uint8_t data[] = {0x92};
+    this->send(data, 1);
+}
+
+void MksServoMotor::send_angle_error_read() {
+    uint8_t data[] = {0x39};
+    this->send(data, 1);
+    this->angle_error_read_pending = true;
+    this->angle_error_read_received = false;
+    this->angle_error_read_sent_at = millis();
+}
+
+void MksServoMotor::step() {
+    Module::step();
+
+    if (this->pz_state != PZ_IDLE) {
+        this->step_precision_zero();
+    }
+}
+
+void MksServoMotor::step_precision_zero() {
+    switch (this->pz_state) {
+    case PZ_FIRST_ROTATE:
+        this->send_working_current(1000);
+        this->send_rotate(this->pz_target_degrees, this->pz_speed, this->pz_acc);
+        this->pz_phase_start = millis();
+        this->pz_state = PZ_WAIT_FIRST_ROTATE;
+        break;
+    case PZ_WAIT_FIRST_ROTATE:
+        if (millis_since(this->pz_phase_start) >= this->pz_rotate_wait_ms) {
+            this->pz_state = PZ_READ_ERROR;
+        }
+        break;
+    case PZ_READ_ERROR:
+        this->angle_error_read_retries = 0;
+        this->send_angle_error_read();
+        this->pz_state = PZ_WAIT_ERROR;
+        break;
+    case PZ_WAIT_ERROR:
+        if (this->angle_error_read_received) {
+            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);
+            this->pz_phase_start = millis();
+            this->pz_state = PZ_WAIT_CORRECT_ROTATE;
+        } else if (millis_since(this->angle_error_read_sent_at) > 200) {
+            if (this->angle_error_read_retries < 5) {
+                this->angle_error_read_retries++;
+                this->send_angle_error_read();
+            } else {
+                this->pz_state = PZ_FAILED;
+                this->properties.at("homing_state")->integer_value = PZ_FAILED;
+            }
+        }
+        break;
+    case PZ_WAIT_CORRECT_ROTATE:
+        if (millis_since(this->pz_phase_start) >= this->pz_correct_wait_ms) {
+            this->pz_state = PZ_SET_ZERO;
+        }
+        break;
+    case PZ_SET_ZERO:
+        this->send_coord_zero();
+        this->properties.at("position")->number_value = 0.0;
+        this->pz_phase_start = millis();
+        this->pz_state = PZ_WAIT_AFTER_ZERO;
+        break;
+    case PZ_WAIT_AFTER_ZERO:
+        if (millis_since(this->pz_phase_start) >= this->pz_wait_after_zero_ms) {
+            this->pz_state = PZ_MOVE_TO_START;
+        }
+        break;
+    case PZ_MOVE_TO_START:
+        this->send_rotate(-110.0, this->pz_speed, this->pz_acc);
+        this->properties.at("position")->number_value = -110.0;
+        this->pz_state = PZ_DONE;
+        this->properties.at("homing_state")->integer_value = PZ_DONE;
+        this->properties.at("homing_active")->boolean_value = false;
+        break;
+    default:
+        break;
+    }
+}
+
+void MksServoMotor::call(const std::string method_name, const std::vector<ConstExpression_ptr> arguments) {
+    if (method_name == "enable") {
+        Module::expect(arguments, 0);
+        this->send_enable(true);
+    } else if (method_name == "disable") {
+        Module::expect(arguments, 0);
+        this->send_enable(false);
+    } else if (method_name == "set_vfoc") {
+        Module::expect(arguments, 0);
+        this->send_set_vfoc();
+    } else if (method_name == "zero") {
+        Module::expect(arguments, 0);
+        this->send_coord_zero();
+        this->properties.at("position")->number_value = 0.0;
+    } else if (method_name == "precision_zero") {
+        Module::expect(arguments, 0);
+        this->pz_state = PZ_FIRST_ROTATE;
+        this->properties.at("homing_active")->boolean_value = true;
+        this->properties.at("homing_state")->integer_value = PZ_FIRST_ROTATE;
+    } else if (method_name == "set_working_current") {
+        Module::expect(arguments, 1, integer);
+        this->send_working_current(arguments[0]->evaluate_integer());
+    } else if (method_name == "set_holding_current") {
+        Module::expect(arguments, 1, integer);
+        this->send_holding_current(arguments[0]->evaluate_integer());
+    } else if (method_name == "run") {
+        Module::expect(arguments, 3, integer, integer, integer);
+        int64_t speed = arguments[0]->evaluate_integer();
+        int64_t direction = arguments[1]->evaluate_integer();
+        int64_t acc = arguments[2]->evaluate_integer();
+        this->send_run_internal(direction, speed, acc);
+        this->properties.at("speed")->integer_value = speed;
+    } else if (method_name == "stop") {
+        Module::expect(arguments, 1, integer);
+        this->send_stop_internal(arguments[0]->evaluate_integer());
+        this->properties.at("speed")->integer_value = 0;
+    } else if (method_name == "grip") {
+        Module::expect(arguments, 0);
+        this->send_rotate(5.0, 3000, MAX_ACC);
+        this->properties.at("position")->number_value = 5.0;
+        this->properties.at("speed")->integer_value = 3000;
+    } else if (method_name == "release") {
+        Module::expect(arguments, 0);
+        this->send_rotate(-40.0, 3000, MAX_ACC);
+        this->properties.at("position")->number_value = -40.0;
+        this->properties.at("speed")->integer_value = 3000;
+    } else if (method_name == "rotate") {
+        Module::expect(arguments, 3, numbery, integer, integer);
+        double degrees = arguments[0]->evaluate_number();
+        int64_t speed = arguments[1]->evaluate_integer();
+        int64_t acc = arguments[2]->evaluate_integer();
+        this->send_rotate(degrees, speed, acc);
+        this->properties.at("position")->number_value = degrees;
+        this->properties.at("speed")->integer_value = speed;
+    } else {
+        Module::call(method_name, arguments);
+    }
+}
+
+void MksServoMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) {
+    if (count < 1) {
+        return;
+    }
+    if (data[0] == 0x39 && count == 6) {
+        // CRC validation
+        uint8_t crc = (uint8_t)(this->can_id & 0xFF);
+        for (int i = 0; i < 5; i++) {
+            crc += data[i];
+        }
+        if ((crc & 0xFF) != data[5]) {
+            return;
+        }
+        // Extract 32-bit big-endian signed angle error from data[1..4]
+        int32_t val = ((int32_t)data[1] << 24) |
+                      ((int32_t)data[2] << 16) |
+                      ((int32_t)data[3] << 8) |
+                      (int32_t)data[4];
+        this->angle_error_value = val;
+        this->angle_error_read_received = true;
+        this->angle_error_read_pending = false;
+    }
+}