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:
parent
b353a2ba29
commit
021caef61a
@ -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
|
|
||||||
|
|||||||
@ -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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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 ── */
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user