diff --git a/include/config.h b/include/config.h index 28671cd..293fe7e 100644 --- a/include/config.h +++ b/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 diff --git a/include/motor_driver.h b/include/motor_driver.h new file mode 100644 index 0000000..30c4ae2 --- /dev/null +++ b/include/motor_driver.h @@ -0,0 +1,47 @@ +#ifndef MOTOR_DRIVER_H +#define MOTOR_DRIVER_H + +#include +#include + +/* + * 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 diff --git a/src/main.c b/src/main.c index e18f36f..45516c1 100644 --- a/src/main.c +++ b/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); } } diff --git a/src/motor_driver.c b/src/motor_driver.c new file mode 100644 index 0000000..2bddc48 --- /dev/null +++ b/src/motor_driver.c @@ -0,0 +1,58 @@ +#include "motor_driver.h" +#include "hoverboard.h" +#include "config.h" +#include + +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; +}