Merge pull request 'feat: motor driver layer — differential drive, steer ramp, estop' (#19) from sl-controls/motor-driver into main

include/config.h

@@ -140,4 +140,8 @@
 #define MAX_SPEED_LIMIT     100
 #define WATCHDOG_TIMEOUT_MS 50
 
+// --- Motor Driver ---
+#define MOTOR_CMD_MAX           1000    /* ESC range: -1000..+1000 */
+#define MOTOR_STEER_RAMP_RATE   20      /* counts/ms — steer ramp only */
+
 #endif // CONFIG_H

include/motor_driver.h

@@ -0,0 +1,47 @@
+#ifndef MOTOR_DRIVER_H
+#define MOTOR_DRIVER_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * SaltyLab Motor Driver
+ *
+ * Sits between the balance PID and the raw hoverboard UART driver.
+ * Responsibilities:
+ *   - Differential drive mixing: speed = balance_cmd, steer mixed in
+ *   - Steer ramping to avoid sudden yaw torque disturbing balance
+ *   - Headroom clamping: |speed| + |steer| <= MOTOR_CMD_MAX
+ *   - Emergency stop: immediate zero + latch until explicitly cleared
+ *
+ * Balance PID output is NOT ramped — it needs full immediate authority.
+ * Only the steer channel is ramped.
+ *
+ * Call motor_driver_update() at the ESC send rate (50Hz / every 20ms).
+ */
+
+typedef struct {
+    int16_t steer_actual;   /* Ramped steer command currently sent */
+    bool    estop;          /* Emergency stop latched */
+} motor_driver_t;
+
+void motor_driver_init(motor_driver_t *m);
+
+/*
+ * Update and send to ESC.
+ *   balance_cmd : PID output, -1000..+1000
+ *   steer_cmd   : desired yaw/steer, -1000..+1000 (future RC/autonomous input)
+ *   now         : HAL_GetTick() timestamp (ms) for ramp delta-time
+ */
+void motor_driver_update(motor_driver_t *m,
+                         int16_t balance_cmd,
+                         int16_t steer_cmd,
+                         uint32_t now);
+
+/* Latch emergency stop — sends zero immediately */
+void motor_driver_estop(motor_driver_t *m);
+
+/* Clear emergency stop latch (only call from armed/ready context) */
+void motor_driver_estop_clear(motor_driver_t *m);
+
+#endif

src/main.c

@@ -6,6 +6,7 @@
 #include "mpu6000.h"
 #include "balance.h"
 #include "hoverboard.h"
+#include "motor_driver.h"
 #include "config.h"
 #include "status.h"
 #include "safety.h"
@@ -114,6 +115,10 @@ int main(void) {
     balance_t bal;
     balance_init(&bal);
 
+    /* Init motor driver */
+    motor_driver_t motors;
+    motor_driver_init(&motors);
+
     /*
      * IWDG starts AFTER all peripheral inits — avoids reset during mpu6000_init()
      * which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS).
@@ -181,14 +186,21 @@ int main(void) {
             balance_update(&bal, &imu, dt);
         }
 
-        /* Send to hoverboard ESC at 50Hz (every 20ms)
-         * Both wheels get same speed for balance, steer=0 for now */
+        /* Latch estop on tilt fault or disarm */
+        if (bal.state == BALANCE_TILT_FAULT) {
+            motor_driver_estop(&motors);
+        } else if (bal.state == BALANCE_DISARMED && motors.estop) {
+            motor_driver_estop_clear(&motors);
+        }
+
+        /* Send to hoverboard ESC at 50Hz (every 20ms) */
         if (now - esc_tick >= 20) {
             esc_tick = now;
             if (bal.state == BALANCE_ARMED) {
-                hoverboard_send(bal.motor_cmd, 0);
+                motor_driver_update(&motors, bal.motor_cmd, 0, now);
             } else {
-                hoverboard_send(0, 0);  /* Always send to prevent ESC timeout */
+                /* Always send zero while disarmed to prevent ESC timeout */
+                motor_driver_update(&motors, 0, 0, now);
             }
         }
 

src/motor_driver.c

@@ -0,0 +1,58 @@
+#include "motor_driver.h"
+#include "hoverboard.h"
+#include "config.h"
+#include <stdlib.h>
+
+void motor_driver_init(motor_driver_t *m) {
+    m->steer_actual = 0;
+    m->estop        = false;
+}
+
+void motor_driver_update(motor_driver_t *m,
+                         int16_t balance_cmd,
+                         int16_t steer_cmd,
+                         uint32_t now) {
+    static uint32_t s_last_tick = 0;
+
+    /* Delta-time for ramp (cap at 100ms to handle first call / long gaps).
+     * Always update tick so ramp starts fresh when estop clears. */
+    int32_t dt_ms = (int32_t)(now - s_last_tick);
+    if (dt_ms > 100) dt_ms = 100;
+    s_last_tick = now;
+
+    /* Emergency stop: send zero, hold latch */
+    if (m->estop) {
+        hoverboard_send(0, 0);
+        return;
+    }
+
+    /* Ramp steer toward target at MOTOR_STEER_RAMP_RATE counts/ms */
+    int32_t max_step = (int32_t)MOTOR_STEER_RAMP_RATE * dt_ms;
+    int32_t steer_error = (int32_t)steer_cmd - (int32_t)m->steer_actual;
+    if (steer_error > max_step)       steer_error = max_step;
+    else if (steer_error < -max_step) steer_error = -max_step;
+    m->steer_actual = (int16_t)((int32_t)m->steer_actual + steer_error);
+
+    /* Headroom clamp: ensure |speed| + |steer| <= MOTOR_CMD_MAX
+     * Reduce steer (not balance) to preserve PID authority. */
+    int32_t speed = (int32_t)balance_cmd;
+    int32_t steer = (int32_t)m->steer_actual;
+    int32_t headroom = MOTOR_CMD_MAX - abs((int)speed);
+    if (headroom < 0) headroom = 0;
+    if (steer >  headroom) steer =  headroom;
+    if (steer < -headroom) steer = -headroom;
+
+    hoverboard_send((int16_t)speed, (int16_t)steer);
+}
+
+void motor_driver_estop(motor_driver_t *m) {
+    m->estop        = true;
+    m->steer_actual = 0;
+    /* Don't call hoverboard_send here — caller must drive sends via
+     * motor_driver_update() at the normal 50Hz ESC rate to avoid
+     * flooding the UART with 1kHz zero packets. */
+}
+
+void motor_driver_estop_clear(motor_driver_t *m) {
+    m->estop = false;
+}