refactor(balance): remove balance-bot safety cutoffs, front VESC drive only

Robot is no longer a balance bot. Remove:
- TILT_CUTOFF_DEG (±25° tilt cutoff) from config.h
- BAL_TILT_FAULT enum value from orin_serial.h
- CMD_PID / orin_pid_t (balance PID tuning) from protocol
- Steer differential (RPM_PER_STEER_UNIT) from drive task
- VESC_ID_B drive command — front VESC (ID 61) only

Update VESC CAN IDs: 56→61 (front), 68→79 (rear).
VESC_ID_B retained for rear telemetry RX only.
ESTOP and heartbeat watchdog unchanged.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-firmware 2026-04-19 13:11:13 -04:00
parent b353a2ba29
commit 021caef61a
4 changed files with 14 additions and 52 deletions

View File

@ -24,9 +24,9 @@
#define VESC_CAN_RX_GPIO 1 /* SN65HVD230 RXD → ESP32 TWAI RX */ #define VESC_CAN_RX_GPIO 1 /* SN65HVD230 RXD → ESP32 TWAI RX */
#define VESC_CAN_RX_QUEUE 32 #define VESC_CAN_RX_QUEUE 32
/* VESC node IDs — matched to bd-wim1 TELEM_VESC_LEFT/RIGHT mapping */ /* VESC node IDs */
#define VESC_ID_A 56u /* TELEM_VESC_LEFT (0x81) */ #define VESC_ID_A 61u /* FRONT VESC — drive + telemetry (0x81) */
#define VESC_ID_B 68u /* TELEM_VESC_RIGHT (0x82) */ #define VESC_ID_B 79u /* REAR VESC — telemetry only (0x82) */
/* ── Safety / timing ── */ /* ── Safety / timing ── */
#define HB_TIMEOUT_MS 500u /* heartbeat watchdog: disarm if exceeded */ #define HB_TIMEOUT_MS 500u /* heartbeat watchdog: disarm if exceeded */
@ -36,7 +36,3 @@
/* ── Drive → VESC RPM scaling ── */ /* ── Drive → VESC RPM scaling ── */
#define RPM_PER_SPEED_UNIT 5 /* speed_units=1000 → 5000 ERPM */ #define RPM_PER_SPEED_UNIT 5 /* speed_units=1000 → 5000 ERPM */
#define RPM_PER_STEER_UNIT 3 /* steer differential scale */
/* ── Tilt cutoff ── */
#define TILT_CUTOFF_DEG 25.0f

View File

@ -63,20 +63,14 @@ static void drive_task(void *arg)
bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS; bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
bool drive_stale = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS; bool drive_stale = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS;
int32_t left_erpm = 0; int32_t front_erpm = 0;
int32_t right_erpm = 0;
if (g_orin_ctrl.armed && !g_orin_ctrl.estop && if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
!hb_timeout && !drive_stale) { !hb_timeout && !drive_stale) {
int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT; front_erpm = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT;
left_erpm = spd + str;
right_erpm = spd - str;
} }
/* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */ vesc_can_send_rpm(VESC_ID_A, front_erpm);
vesc_can_send_rpm(VESC_ID_A, left_erpm);
vesc_can_send_rpm(VESC_ID_B, right_erpm);
} }
} }

View File

@ -17,9 +17,8 @@
static const char *TAG = "orin"; static const char *TAG = "orin";
/* ── Shared state ── */ /* ── Shared state ── */
orin_drive_t g_orin_drive = {0}; orin_drive_t g_orin_drive = {0};
orin_pid_t g_orin_pid = {0}; orin_control_t g_orin_ctrl = {.armed = false, .estop = false, .hb_last_ms = 0};
orin_control_t g_orin_ctrl = {.armed = false, .estop = false, .hb_last_ms = 0};
/* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */ /* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */
static uint8_t crc8(const uint8_t *data, uint8_t len) static uint8_t crc8(const uint8_t *data, uint8_t len)
@ -188,25 +187,6 @@ static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len,
orin_send_ack(tx_q, type); orin_send_ack(tx_q, type);
break; break;
case CMD_PID:
if (len < 12u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
/* float32 big-endian: copy and swap bytes */
{
uint32_t raw;
raw = ((uint32_t)payload[0] << 24u) | ((uint32_t)payload[1] << 16u) |
((uint32_t)payload[2] << 8u) | (uint32_t)payload[3];
memcpy((void*)&g_orin_pid.kp, &raw, 4u);
raw = ((uint32_t)payload[4] << 24u) | ((uint32_t)payload[5] << 16u) |
((uint32_t)payload[6] << 8u) | (uint32_t)payload[7];
memcpy((void*)&g_orin_pid.ki, &raw, 4u);
raw = ((uint32_t)payload[8] << 24u) | ((uint32_t)payload[9] << 16u) |
((uint32_t)payload[10] << 8u) | (uint32_t)payload[11];
memcpy((void*)&g_orin_pid.kd, &raw, 4u);
g_orin_pid.updated = true;
}
orin_send_ack(tx_q, type);
break;
case CMD_OTA_CHECK: case CMD_OTA_CHECK:
/* Trigger an immediate Gitea version check */ /* Trigger an immediate Gitea version check */
gitea_ota_check_now(); gitea_ota_check_now();

View File

@ -23,12 +23,11 @@
#define CMD_DRIVE 0x02u /* int16 speed + int16 steer, BE */ #define CMD_DRIVE 0x02u /* int16 speed + int16 steer, BE */
#define CMD_ESTOP 0x03u /* uint8: 1=assert, 0=clear */ #define CMD_ESTOP 0x03u /* uint8: 1=assert, 0=clear */
#define CMD_ARM 0x04u /* uint8: 1=arm, 0=disarm */ #define CMD_ARM 0x04u /* uint8: 1=arm, 0=disarm */
#define CMD_PID 0x05u /* float32 kp, ki, kd, BE */
/* ── Telemetry types: ESP32 → Orin ── */ /* ── Telemetry types: ESP32 → Orin ── */
#define TELEM_STATUS 0x80u /* status @ 10 Hz */ #define TELEM_STATUS 0x80u /* status @ 10 Hz */
#define TELEM_VESC_LEFT 0x81u /* VESC ID 56 telemetry @ 10 Hz */ #define TELEM_VESC_LEFT 0x81u /* VESC ID 61 (front) telemetry @ 10 Hz */
#define TELEM_VESC_RIGHT 0x82u /* VESC ID 68 telemetry @ 10 Hz */ #define TELEM_VESC_RIGHT 0x82u /* VESC ID 79 (rear) telemetry @ 10 Hz */
#define TELEM_OTA_STATUS 0x83u /* OTA state + progress (bd-1s1s) */ #define TELEM_OTA_STATUS 0x83u /* OTA state + progress (bd-1s1s) */
#define TELEM_VERSION_INFO 0x84u /* firmware version report (bd-1s1s) */ #define TELEM_VERSION_INFO 0x84u /* firmware version report (bd-1s1s) */
#define RESP_ACK 0xA0u #define RESP_ACK 0xA0u
@ -51,12 +50,11 @@
#define ERR_OTA_BUSY 0x05u #define ERR_OTA_BUSY 0x05u
#define ERR_OTA_NO_UPDATE 0x06u #define ERR_OTA_NO_UPDATE 0x06u
/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */ /* ── Drive state (mirrored from TELEM_STATUS.balance_state) ── */
typedef enum { typedef enum {
BAL_DISARMED = 0, BAL_DISARMED = 0,
BAL_ARMED = 1, BAL_ARMED = 1,
BAL_TILT_FAULT = 2, BAL_ESTOP = 3,
BAL_ESTOP = 3,
} bal_state_t; } bal_state_t;
/* ── Shared state written by RX task, consumed by main/vesc tasks ── */ /* ── Shared state written by RX task, consumed by main/vesc tasks ── */
@ -66,11 +64,6 @@ typedef struct {
volatile uint32_t updated_ms; /* esp_timer tick at last CMD_DRIVE */ volatile uint32_t updated_ms; /* esp_timer tick at last CMD_DRIVE */
} orin_drive_t; } orin_drive_t;
typedef struct {
volatile float kp, ki, kd;
volatile bool updated;
} orin_pid_t;
typedef struct { typedef struct {
volatile bool armed; volatile bool armed;
volatile bool estop; volatile bool estop;
@ -86,7 +79,6 @@ typedef struct {
/* ── Globals (defined in orin_serial.c, extern here) ── */ /* ── Globals (defined in orin_serial.c, extern here) ── */
extern orin_drive_t g_orin_drive; extern orin_drive_t g_orin_drive;
extern orin_pid_t g_orin_pid;
extern orin_control_t g_orin_ctrl; extern orin_control_t g_orin_ctrl;
/* ── API ── */ /* ── API ── */