From 21fbc8676012d303b4295e026bcb8727495d4563 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 21:40:25 -0400 Subject: [PATCH 1/4] Balance bot: ESP32 PID balance controller with BLE dashboard MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - packages/pid: standalone ESP-IDF component (generic PID with anti-windup) - packages/sensors: standalone ESP-IDF component (MPU6050 complementary filter) - firmware: balance.c/h, Kconfig.projbuild, 4 new BLE GATT characteristics (BALANCE_CMD, BALANCE_PID, BALANCE_STATE, BALANCE_TARGET); compile-time gated by CONFIG_BALANCE_BOT_ENABLED; goto is a Phase-1 stub pending cv merge - dashboard: balance-bot capability (joypad, PID inputs, I-dump select, goto) - protocol: 4 new UUIDs (da6–da9), regenerated uuids.js/uuids.h/uuids.py Co-Authored-By: Claude Sonnet 4.6 --- firmware/esp32_robot_idf/CMakeLists.txt | 4 + firmware/esp32_robot_idf/main/CMakeLists.txt | 4 + .../esp32_robot_idf/main/Kconfig.projbuild | 43 ++ firmware/esp32_robot_idf/main/app_main.c | 8 + firmware/esp32_robot_idf/main/balance.c | 323 +++++++++++++++ firmware/esp32_robot_idf/main/balance.h | 33 ++ firmware/esp32_robot_idf/main/fw_info.c | 21 + firmware/esp32_robot_idf/main/gatt_svr.c | 99 +++++ firmware/esp32_robot_idf/main/gatt_svr.h | 5 + firmware/esp32_robot_idf/main/uuids.h | 6 +- firmware/pi_robot/uuids.py | 6 +- packages/pid/CMakeLists.txt | 4 + packages/pid/include/pid/pid.h | 34 ++ packages/pid/src/pid.c | 43 ++ packages/sensors/CMakeLists.txt | 5 + packages/sensors/include/sensors/imu.h | 26 ++ packages/sensors/src/imu.c | 130 ++++++ protocol/uuids.json | 6 +- public/ble.js | 8 + public/capabilities/runtime/balance-bot.js | 372 ++++++++++++++++++ public/capabilities/runtime/index.js | 2 + public/styles.css | 53 +++ public/uuids.js | 6 +- 23 files changed, 1237 insertions(+), 4 deletions(-) create mode 100644 firmware/esp32_robot_idf/main/Kconfig.projbuild create mode 100644 firmware/esp32_robot_idf/main/balance.c create mode 100644 firmware/esp32_robot_idf/main/balance.h create mode 100644 packages/pid/CMakeLists.txt create mode 100644 packages/pid/include/pid/pid.h create mode 100644 packages/pid/src/pid.c create mode 100644 packages/sensors/CMakeLists.txt create mode 100644 packages/sensors/include/sensors/imu.h create mode 100644 packages/sensors/src/imu.c create mode 100644 public/capabilities/runtime/balance-bot.js diff --git a/firmware/esp32_robot_idf/CMakeLists.txt b/firmware/esp32_robot_idf/CMakeLists.txt index 7fe5ce01..19c2d4f0 100644 --- a/firmware/esp32_robot_idf/CMakeLists.txt +++ b/firmware/esp32_robot_idf/CMakeLists.txt @@ -34,6 +34,10 @@ if(EXISTS "${MANAGED_COMPONENTS}" AND EXISTS "${PATCH_DIR}") endforeach() endif() +list(APPEND EXTRA_COMPONENT_DIRS + "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/pid" + "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/sensors") + include($ENV{IDF_PATH}/tools/cmake/project.cmake) project(esp32_robot) diff --git a/firmware/esp32_robot_idf/main/CMakeLists.txt b/firmware/esp32_robot_idf/main/CMakeLists.txt index 2369750e..97c42c65 100644 --- a/firmware/esp32_robot_idf/main/CMakeLists.txt +++ b/firmware/esp32_robot_idf/main/CMakeLists.txt @@ -23,6 +23,7 @@ idf_component_register( "telemetry.c" "webrtc_peer.c" "http_stream.c" + "balance.c" INCLUDE_DIRS "." PRIV_REQUIRES @@ -47,4 +48,7 @@ idf_component_register( esp_timer # JSON parsing (cJSON via ESP-IDF's json component) json + # Balance bot packages + pid + sensors ) diff --git a/firmware/esp32_robot_idf/main/Kconfig.projbuild b/firmware/esp32_robot_idf/main/Kconfig.projbuild new file mode 100644 index 00000000..ddd76c9a --- /dev/null +++ b/firmware/esp32_robot_idf/main/Kconfig.projbuild @@ -0,0 +1,43 @@ +menu "Balance Bot" + + config BALANCE_BOT_ENABLED + bool "Enable balance bot (MPU6050 + PID)" + default n + help + Adds IMU polling task, balance PID loop, and four BLE + characteristics: BALANCE_CMD, BALANCE_PID, BALANCE_STATE, + BALANCE_TARGET. The MOTOR_CHAR remains for status reads but + the balance loop owns the actuators. Raw motor writes are + overridden immediately by the 100 Hz control task. + + config BALANCE_BOT_I2C_PORT + int "I2C port number (0 or 1)" + depends on BALANCE_BOT_ENABLED + default 0 + + config BALANCE_BOT_I2C_SDA + int "MPU6050 SDA GPIO" + depends on BALANCE_BOT_ENABLED + default 21 + + config BALANCE_BOT_I2C_SCL + int "MPU6050 SCL GPIO" + depends on BALANCE_BOT_ENABLED + default 22 + + config BALANCE_BOT_IMU_ADDR + hex "MPU6050 I2C address" + depends on BALANCE_BOT_ENABLED + default 0x68 + help + 0x68 when AD0 pin is LOW (default). 0x69 when AD0 is HIGH. + + config BALANCE_BOT_IMU_PITCH_INVERT + bool "Invert pitch sign" + depends on BALANCE_BOT_ENABLED + default n + help + Enable if the robot leans forward but the pitch angle reads + negative. Depends on how the MPU6050 is mounted on the frame. + +endmenu diff --git a/firmware/esp32_robot_idf/main/app_main.c b/firmware/esp32_robot_idf/main/app_main.c index 1a743779..e45bc4d4 100644 --- a/firmware/esp32_robot_idf/main/app_main.c +++ b/firmware/esp32_robot_idf/main/app_main.c @@ -4,6 +4,9 @@ #include "esp_mac.h" #include "nvs_flash.h" +#if CONFIG_BALANCE_BOT_ENABLED +#include "balance.h" +#endif #include "ble_host.h" #include "camera.h" #include "flash.h" @@ -62,6 +65,11 @@ void app_main(void) { flash_init(pins.flash); motors_init(&pins); +#if CONFIG_BALANCE_BOT_ENABLED + // Must run before fw_info_init so balance_enabled() reflects IMU status. + balance_init(); +#endif + // fw-info reflects the cap surface; built once after caps are up. // Changes (camera profile, pin config) reboot, so a fresh boot // rebuilds it. diff --git a/firmware/esp32_robot_idf/main/balance.c b/firmware/esp32_robot_idf/main/balance.c new file mode 100644 index 00000000..30a91d08 --- /dev/null +++ b/firmware/esp32_robot_idf/main/balance.c @@ -0,0 +1,323 @@ +#include "sdkconfig.h" +#if CONFIG_BALANCE_BOT_ENABLED + +#include "balance.h" + +#include +#include +#include + +#include "cJSON.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include "gatt_svr.h" +#include "sensors/imu.h" +#include "motors.h" +#include "pid/pid.h" + +static const char *TAG = "balance"; + +// ── Tuning constants ──────────────────────────────────────────────────────── + +#define BALANCE_TASK_HZ 100 +#define BALANCE_DT_S (1.0f / BALANCE_TASK_HZ) + +// How many degrees of setpoint shift a full joystick deflection produces. +// Small: bot leans gently and moves slowly. Too large: unstable. +#define LEAN_MAX_DEG 5.0f + +// Differential motor units added for turning (out of ±95 total). +#define TURN_MAX_EFFORT 30.0f + +// Safety cutoff — stop motors if |pitch| exceeds this (bot has fallen). +#define FALLEN_THRESHOLD_DEG 45.0f + +// Motor output headroom — keep below 100 so the H-bridge isn't saturated +// while the turn differential is added. +#define MOTOR_LIMIT 95.0f + +// State notify cadence: every N balance ticks (~10 Hz at 100 Hz loop rate). +#define NOTIFY_EVERY_N_TICKS 10 + +// Default PID starting point. These are intentionally conservative; the user +// tunes them live via the dashboard. Kp too high → oscillation. Kd too high +// → amplifies noise. Ki small so windup doesn't fight a fallen bot. +#define DEFAULT_KP 15.0f +#define DEFAULT_KI 0.5f +#define DEFAULT_KD 1.2f + +// ── Shared state (guarded by s_mux) ──────────────────────────────────────── + +static portMUX_TYPE s_mux = portMUX_INITIALIZER_UNLOCKED; + +static float s_lean_setpoint = 0.0f; // degrees; modified by joystick +static float s_turn_effort = 0.0f; // raw motor units; added differentially +static int64_t s_cmd_last_us = 0; // esp_timer_get_time() at last cmd write + +// Joystick command timeout: if no cmd for this long, snap back to neutral. +// Prevents the bot from driving into a wall after BLE disconnects. +#define CMD_TIMEOUT_US (2LL * 1000000LL) + +typedef enum { MODE_BALANCE, MODE_GOTO, MODE_FALLEN } balance_mode_t; +static balance_mode_t s_mode = MODE_BALANCE; + +// Goto target — stored but not acted on until CV branch merges. +static struct { + float x_mm; + float y_mm; + float theta_deg; + bool active; +} s_target; + +// ── PID ───────────────────────────────────────────────────────────────────── + +static pid_t s_pid; + +// ── I-dump timer ───────────────────────────────────────────────────────────── + +static esp_timer_handle_t s_idump_timer = NULL; +static int s_idump_interval_s = 0; + +static void idump_cb(void *arg) { + taskENTER_CRITICAL(&s_mux); + pid_reset_integral(&s_pid); + taskEXIT_CRITICAL(&s_mux); + ESP_LOGI(TAG, "I-dump: integral reset"); +} + +static void set_idump_interval(int interval_s) { + s_idump_interval_s = interval_s; + esp_timer_stop(s_idump_timer); + if (interval_s > 0) { + esp_timer_start_periodic(s_idump_timer, + (int64_t)interval_s * 1000000LL); + } +} + +// ── Cached JSON buffers ────────────────────────────────────────────────────── + +#define PID_JSON_BUF 128 +#define STATE_JSON_BUF 128 + +static char s_pid_json[PID_JSON_BUF]; +static char s_state_json[STATE_JSON_BUF]; + +static void build_pid_json(void) { + float kp, ki, kd; + taskENTER_CRITICAL(&s_mux); + kp = s_pid.kp; + ki = s_pid.ki; + kd = s_pid.kd; + taskEXIT_CRITICAL(&s_mux); + snprintf(s_pid_json, PID_JSON_BUF, + "{\"p\":%.4g,\"i\":%.4g,\"d\":%.4g,\"idump_s\":%d}", + kp, ki, kd, s_idump_interval_s); +} + +static void build_state_json(float pitch, float setpoint, balance_mode_t mode) { + const char *mode_str; + switch (mode) { + case MODE_GOTO: mode_str = "goto"; break; + case MODE_FALLEN: mode_str = "fallen"; break; + default: mode_str = "balance"; break; + } + snprintf(s_state_json, STATE_JSON_BUF, + "{\"pitch\":%.2f,\"sp\":%.2f,\"mode\":\"%s\"}", + pitch, setpoint, mode_str); +} + +// ── Enabled flag ───────────────────────────────────────────────────────────── + +static bool s_enabled = false; +bool balance_enabled(void) { return s_enabled; } + +// ── 100 Hz control task ────────────────────────────────────────────────────── + +static void balance_task(void *arg) { + TickType_t last_wake = xTaskGetTickCount(); + const TickType_t period = pdMS_TO_TICKS(1000 / BALANCE_TASK_HZ); + int notify_counter = 0; + + while (1) { + vTaskDelayUntil(&last_wake, period); + + imu_update(BALANCE_DT_S); + float pitch = imu_pitch_deg(); + + taskENTER_CRITICAL(&s_mux); + float lean = s_lean_setpoint; + float turn = s_turn_effort; + balance_mode_t mode = s_mode; + + // Command timeout — snap to neutral if operator disappeared. + if (s_cmd_last_us > 0 && + esp_timer_get_time() - s_cmd_last_us > CMD_TIMEOUT_US) { + s_lean_setpoint = 0.0f; + s_turn_effort = 0.0f; + lean = turn = 0.0f; + } + taskEXIT_CRITICAL(&s_mux); + + // Safety: stop and mark fallen if tipped past recovery angle. + if (fabsf(pitch) > FALLEN_THRESHOLD_DEG) { + motors_apply(0, 0); + taskENTER_CRITICAL(&s_mux); + s_mode = MODE_FALLEN; + taskEXIT_CRITICAL(&s_mux); + mode = MODE_FALLEN; + goto notify_check; + } + + // If we were fallen and recovered (someone picked the bot up), + // clear the state and reset the PID. + if (mode == MODE_FALLEN) { + taskENTER_CRITICAL(&s_mux); + s_mode = MODE_BALANCE; + pid_reset_integral(&s_pid); + taskEXIT_CRITICAL(&s_mux); + mode = MODE_BALANCE; + } + + { + // Compute balance effort. + taskENTER_CRITICAL(&s_mux); + float effort = pid_compute(&s_pid, lean - pitch, BALANCE_DT_S); + taskEXIT_CRITICAL(&s_mux); + + // Clamp total effort before adding turn differential. + float effort_clamped = effort; + if (effort_clamped > MOTOR_LIMIT) effort_clamped = MOTOR_LIMIT; + if (effort_clamped < -MOTOR_LIMIT) effort_clamped = -MOTOR_LIMIT; + + float left = effort_clamped + turn; + float right = effort_clamped - turn; + if (left > MOTOR_LIMIT) left = MOTOR_LIMIT; + if (left < -MOTOR_LIMIT) left = -MOTOR_LIMIT; + if (right > MOTOR_LIMIT) right = MOTOR_LIMIT; + if (right < -MOTOR_LIMIT) right = -MOTOR_LIMIT; + + motors_apply((int8_t)left, (int8_t)right); + } + +notify_check: + if (++notify_counter >= NOTIFY_EVERY_N_TICKS) { + notify_counter = 0; + build_state_json(pitch, lean, mode); + gatt_svr_notify_balance_state(); + } + } +} + +// ── Public API ─────────────────────────────────────────────────────────────── + +void balance_init(void) { + // Create I-dump timer (stopped; started when user sets idump_s > 0). + esp_timer_create_args_t ta = { .callback = idump_cb, .name = "idump" }; + esp_timer_create(&ta, &s_idump_timer); + + esp_err_t err = imu_init( + (i2c_port_t)CONFIG_BALANCE_BOT_I2C_PORT, + CONFIG_BALANCE_BOT_I2C_SDA, + CONFIG_BALANCE_BOT_I2C_SCL, + (uint8_t)CONFIG_BALANCE_BOT_IMU_ADDR + ); + if (err != ESP_OK) { + ESP_LOGE(TAG, "IMU init failed — balance disabled"); + return; + } + + pid_init(&s_pid, DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, MOTOR_LIMIT); + build_pid_json(); + build_state_json(imu_pitch_deg(), 0.0f, MODE_BALANCE); + + s_enabled = true; + + // Pin to core 1; NimBLE host runs on core 0 by default. + xTaskCreatePinnedToCore(balance_task, "balance", 4096, NULL, 5, NULL, 1); + ESP_LOGI(TAG, "balance task started at %d Hz", BALANCE_TASK_HZ); +} + +void balance_handle_cmd(int8_t lean_pct, int8_t turn_pct) { + float lean = (lean_pct / 100.0f) * LEAN_MAX_DEG; + float turn = (turn_pct / 100.0f) * TURN_MAX_EFFORT; + taskENTER_CRITICAL(&s_mux); + s_lean_setpoint = lean; + s_turn_effort = turn; + s_cmd_last_us = esp_timer_get_time(); + taskEXIT_CRITICAL(&s_mux); +} + +void balance_handle_pid_write(const uint8_t *data, uint16_t len) { + if (len >= 128) return; + char buf[129]; + memcpy(buf, data, len); + buf[len] = '\0'; + + cJSON *obj = cJSON_Parse(buf); + if (!obj) return; + + cJSON *p = cJSON_GetObjectItemCaseSensitive(obj, "p"); + cJSON *i = cJSON_GetObjectItemCaseSensitive(obj, "i"); + cJSON *d = cJSON_GetObjectItemCaseSensitive(obj, "d"); + cJSON *idump = cJSON_GetObjectItemCaseSensitive(obj, "idump_s"); + + taskENTER_CRITICAL(&s_mux); + if (cJSON_IsNumber(p)) s_pid.kp = (float)p->valuedouble; + if (cJSON_IsNumber(i)) s_pid.ki = (float)i->valuedouble; + if (cJSON_IsNumber(d)) s_pid.kd = (float)d->valuedouble; + taskEXIT_CRITICAL(&s_mux); + + if (cJSON_IsNumber(idump)) { + int iv = idump->valueint; + if (iv < 0) iv = 0; + set_idump_interval(iv); + } + + cJSON_Delete(obj); + + build_pid_json(); + gatt_svr_notify_balance_pid(); + ESP_LOGI(TAG, "PID updated: %s", s_pid_json); +} + +void balance_handle_target_write(const uint8_t *data, uint16_t len) { + if (len >= 128) return; + char buf[129]; + memcpy(buf, data, len); + buf[len] = '\0'; + + cJSON *obj = cJSON_Parse(buf); + if (!obj) return; + + cJSON *x = cJSON_GetObjectItemCaseSensitive(obj, "x"); + cJSON *y = cJSON_GetObjectItemCaseSensitive(obj, "y"); + cJSON *theta = cJSON_GetObjectItemCaseSensitive(obj, "theta"); + cJSON *active = cJSON_GetObjectItemCaseSensitive(obj, "active"); + + if (cJSON_IsNumber(x)) s_target.x_mm = (float)x->valuedouble; + if (cJSON_IsNumber(y)) s_target.y_mm = (float)y->valuedouble; + if (cJSON_IsNumber(theta)) s_target.theta_deg = (float)theta->valuedouble; + + if (cJSON_IsBool(active)) { + s_target.active = cJSON_IsTrue(active); + // Phase 1 stub: accept the target but stay in balance-only mode. + // The goto controller hooks in here once the cv branch merges and + // provides a live cvPosition fix. + taskENTER_CRITICAL(&s_mux); + s_mode = s_target.active ? MODE_GOTO : MODE_BALANCE; + taskEXIT_CRITICAL(&s_mux); + } + + cJSON_Delete(obj); + ESP_LOGI(TAG, "goto target: x=%.0f y=%.0f theta=%.0f active=%d", + s_target.x_mm, s_target.y_mm, s_target.theta_deg, + (int)s_target.active); +} + +const char *balance_pid_json(void) { return s_pid_json; } +const char *balance_state_json(void) { return s_state_json; } + +#endif // CONFIG_BALANCE_BOT_ENABLED diff --git a/firmware/esp32_robot_idf/main/balance.h b/firmware/esp32_robot_idf/main/balance.h new file mode 100644 index 00000000..c6da3ffc --- /dev/null +++ b/firmware/esp32_robot_idf/main/balance.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +// Balance bot state machine + 100 Hz FreeRTOS task. +// +// Control layers (inner to outer): +// 1. Balance PID — closes on IMU pitch; output = raw motor effort. +// 2. Drive layer — joystick lean% shifts the balance setpoint, creating +// a controlled forward/backward tilt. +// 3. Turn layer — joystick turn% adds a signed differential on top of +// the balance output, independent of lean. +// +// Goto mode is a Phase-1 stub: the BLE characteristic is fully wired and +// the target is stored, but the controller stays in balance-only mode until +// CV localization (cv branch) provides a position fix. +// +// Thread safety: lean_setpoint / turn_effort / pid gains are updated from +// the NimBLE host task and read from the balance task on core 1. A +// portMUX_TYPE spinlock guards every cross-task write/read pair. + +void balance_init(void); + +// BLE characteristic handlers — called from gatt_svr.c. +void balance_handle_cmd(int8_t lean_pct, int8_t turn_pct); +void balance_handle_pid_write(const uint8_t *json, uint16_t len); +void balance_handle_target_write(const uint8_t *json, uint16_t len); +const char *balance_pid_json(void); +const char *balance_state_json(void); + +// True once imu_init succeeded and the task is running. +bool balance_enabled(void); diff --git a/firmware/esp32_robot_idf/main/fw_info.c b/firmware/esp32_robot_idf/main/fw_info.c index 52cfcafa..134f8919 100644 --- a/firmware/esp32_robot_idf/main/fw_info.c +++ b/firmware/esp32_robot_idf/main/fw_info.c @@ -9,6 +9,9 @@ #include "flash.h" #include "led.h" #include "motors.h" +#if CONFIG_BALANCE_BOT_ENABLED +#include "balance.h" +#endif static const char *TAG = "fw_info"; @@ -45,6 +48,23 @@ void fw_info_init(const pin_config_t *pins) { "%s{\"name\":\"wifi\",\"type\":\"wifi-scan\"}", first ? "" : ","); first = false; +#if CONFIG_BALANCE_BOT_ENABLED + if (balance_enabled()) { + // Balance bot: advertise "balance-bot" instead of raw motor access. + // The four BALANCE_* characteristics handle control; MOTOR_CHAR + // stays in the GATT table for status reads but isn't a user-facing cap. + o += snprintf(s_buf + o, FW_INFO_BUF_SIZE - o, + ",{\"name\":\"balance-bot\",\"type\":\"balance-bot\"}"); + } else if (motors_enabled()) { + // IMU init failed — degrade gracefully to plain motor control. + o += snprintf(s_buf + o, FW_INFO_BUF_SIZE - o, + ",{\"name\":\"motors\",\"type\":\"signed-pair\",\"range\":[-100,100]," + "\"pins\":{\"left\":{\"forward\":%d,\"backward\":%d}," + "\"right\":{\"forward\":%d,\"backward\":%d}}}", + pins->motor_l_fwd, pins->motor_l_bwd, + pins->motor_r_fwd, pins->motor_r_bwd); + } +#else if (motors_enabled()) { o += snprintf(s_buf + o, FW_INFO_BUF_SIZE - o, ",{\"name\":\"motors\",\"type\":\"signed-pair\",\"range\":[-100,100]," @@ -53,6 +73,7 @@ void fw_info_init(const pin_config_t *pins) { pins->motor_l_fwd, pins->motor_l_bwd, pins->motor_r_fwd, pins->motor_r_bwd); } +#endif if (camera_ready()) { o += snprintf(s_buf + o, FW_INFO_BUF_SIZE - o, ",{\"name\":\"camera\",\"type\":\"mjpeg-stream\"}"); diff --git a/firmware/esp32_robot_idf/main/gatt_svr.c b/firmware/esp32_robot_idf/main/gatt_svr.c index e199aaba..7d6daad5 100644 --- a/firmware/esp32_robot_idf/main/gatt_svr.c +++ b/firmware/esp32_robot_idf/main/gatt_svr.c @@ -20,6 +20,9 @@ #include "uuids.h" #include "webrtc_peer.h" #include "wifi_sta.h" +#if CONFIG_BALANCE_BOT_ENABLED +#include "balance.h" +#endif static const char *TAG = "gatt_svr"; @@ -38,6 +41,14 @@ static ble_uuid128_t s_snapshot_data_uuid; static ble_uuid128_t s_telemetry_uuid; static ble_uuid128_t s_fw_info_uuid; static ble_uuid128_t s_signal_uuid; +#if CONFIG_BALANCE_BOT_ENABLED +static ble_uuid128_t s_balance_cmd_uuid; +static ble_uuid128_t s_balance_pid_uuid; +static ble_uuid128_t s_balance_state_uuid; +static ble_uuid128_t s_balance_target_uuid; +static uint16_t s_balance_pid_handle; +static uint16_t s_balance_state_handle; +#endif static uint16_t s_led_handle; static uint16_t s_flash_handle; @@ -251,6 +262,59 @@ static int signal_access(uint16_t conn, uint16_t attr, return BLE_ATT_ERR_UNLIKELY; } +#if CONFIG_BALANCE_BOT_ENABLED +static int balance_cmd_access(uint16_t conn, uint16_t attr, + struct ble_gatt_access_ctxt *ctxt, void *arg) { + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + uint8_t buf[2] = {0}; + uint16_t copied = 0; + ble_hs_mbuf_to_flat(ctxt->om, buf, sizeof(buf), &copied); + if (copied >= 2) balance_handle_cmd((int8_t)buf[0], (int8_t)buf[1]); + return 0; + } + return BLE_ATT_ERR_UNLIKELY; +} + +static int balance_pid_access(uint16_t conn, uint16_t attr, + struct ble_gatt_access_ctxt *ctxt, void *arg) { + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + uint8_t buf[128]; + uint16_t copied = 0; + ble_hs_mbuf_to_flat(ctxt->om, buf, sizeof(buf), &copied); + if (copied > 0) balance_handle_pid_write(buf, copied); + return 0; + } + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + const char *json = balance_pid_json(); + return os_mbuf_append(ctxt->om, json, strlen(json)) == 0 + ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + return BLE_ATT_ERR_UNLIKELY; +} + +static int balance_state_access(uint16_t conn, uint16_t attr, + struct ble_gatt_access_ctxt *ctxt, void *arg) { + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + const char *json = balance_state_json(); + return os_mbuf_append(ctxt->om, json, strlen(json)) == 0 + ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + return BLE_ATT_ERR_UNLIKELY; +} + +static int balance_target_access(uint16_t conn, uint16_t attr, + struct ble_gatt_access_ctxt *ctxt, void *arg) { + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + uint8_t buf[128]; + uint16_t copied = 0; + ble_hs_mbuf_to_flat(ctxt->om, buf, sizeof(buf), &copied); + if (copied > 0) balance_handle_target_write(buf, copied); + return 0; + } + return BLE_ATT_ERR_UNLIKELY; +} +#endif + static const struct ble_gatt_chr_def s_chars[] = { { .uuid = &s_led_uuid.u, @@ -334,6 +398,30 @@ static const struct ble_gatt_chr_def s_chars[] = { .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY, .val_handle = &s_signal_handle, }, +#if CONFIG_BALANCE_BOT_ENABLED + { + .uuid = &s_balance_cmd_uuid.u, + .access_cb = balance_cmd_access, + .flags = BLE_GATT_CHR_F_WRITE, + }, + { + .uuid = &s_balance_pid_uuid.u, + .access_cb = balance_pid_access, + .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY, + .val_handle = &s_balance_pid_handle, + }, + { + .uuid = &s_balance_state_uuid.u, + .access_cb = balance_state_access, + .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY, + .val_handle = &s_balance_state_handle, + }, + { + .uuid = &s_balance_target_uuid.u, + .access_cb = balance_target_access, + .flags = BLE_GATT_CHR_F_WRITE, + }, +#endif { 0 }, }; @@ -362,6 +450,12 @@ void gatt_svr_init(void) { parse_uuid128(TELEMETRY_CHAR_UUID, &s_telemetry_uuid); parse_uuid128(FW_INFO_CHAR_UUID, &s_fw_info_uuid); parse_uuid128(SIGNAL_CHAR_UUID, &s_signal_uuid); +#if CONFIG_BALANCE_BOT_ENABLED + parse_uuid128(BALANCE_CMD_CHAR_UUID, &s_balance_cmd_uuid); + parse_uuid128(BALANCE_PID_CHAR_UUID, &s_balance_pid_uuid); + parse_uuid128(BALANCE_STATE_CHAR_UUID, &s_balance_state_uuid); + parse_uuid128(BALANCE_TARGET_CHAR_UUID, &s_balance_target_uuid); +#endif int rc = ble_gatts_count_cfg(s_svcs); if (rc != 0) { ESP_LOGE(TAG, "count_cfg rc=%d", rc); return; } @@ -379,6 +473,11 @@ void gatt_svr_notify_ota_status(void) { if (s_ota_status_handle) ble_gatts_chr void gatt_svr_notify_telemetry(void) { if (s_telemetry_handle) ble_gatts_chr_updated(s_telemetry_handle); } void gatt_svr_notify_fw_info(void) { if (s_fw_info_handle) ble_gatts_chr_updated(s_fw_info_handle); } +#if CONFIG_BALANCE_BOT_ENABLED +void gatt_svr_notify_balance_pid(void) { if (s_balance_pid_handle) ble_gatts_chr_updated(s_balance_pid_handle); } +void gatt_svr_notify_balance_state(void) { if (s_balance_state_handle) ble_gatts_chr_updated(s_balance_state_handle); } +#endif + void gatt_svr_snapshot_send(const uint8_t *buf, size_t len) { uint16_t conn = ble_host_active_conn(); if (conn == BLE_HS_CONN_HANDLE_NONE) return; diff --git a/firmware/esp32_robot_idf/main/gatt_svr.h b/firmware/esp32_robot_idf/main/gatt_svr.h index 34d127fe..9d65fd39 100644 --- a/firmware/esp32_robot_idf/main/gatt_svr.h +++ b/firmware/esp32_robot_idf/main/gatt_svr.h @@ -26,6 +26,11 @@ void gatt_svr_notify_ota_status(void); void gatt_svr_notify_telemetry(void); void gatt_svr_notify_fw_info(void); +#if CONFIG_BALANCE_BOT_ENABLED +void gatt_svr_notify_balance_pid(void); +void gatt_svr_notify_balance_state(void); +#endif + // Push a snapshot frame to the active central. Custom-payload notify // (not a stored-value notify) — wraps ble_gatts_notify_custom. No-op if // no central is connected. The snapshot task drives this directly with diff --git a/firmware/esp32_robot_idf/main/uuids.h b/firmware/esp32_robot_idf/main/uuids.h index 02f3a0dd..75522abf 100644 --- a/firmware/esp32_robot_idf/main/uuids.h +++ b/firmware/esp32_robot_idf/main/uuids.h @@ -1,4 +1,4 @@ -// Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. +// Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. // Run `make gen-uuids` (or `python3 tools/gen-uuids.py`) after editing the JSON. #pragma once @@ -23,6 +23,10 @@ #define FLASH_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3" #define PIN_CONFIG_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4" #define SIGNAL_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5" +#define BALANCE_CMD_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6" +#define BALANCE_PID_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7" +#define BALANCE_STATE_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8" +#define BALANCE_TARGET_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da9" // heartbeat_service #define HEARTBEAT_SVC_UUID "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0" diff --git a/firmware/pi_robot/uuids.py b/firmware/pi_robot/uuids.py index 9e53f6b8..c99506cc 100644 --- a/firmware/pi_robot/uuids.py +++ b/firmware/pi_robot/uuids.py @@ -1,4 +1,4 @@ -# Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. +# Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. # Run `make gen-uuids` (or `python3 tools/gen-uuids.py`) after editing the JSON. # main_service @@ -22,6 +22,10 @@ FLASH_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3" PIN_CONFIG_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4" SIGNAL_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5" +BALANCE_CMD_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6" +BALANCE_PID_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7" +BALANCE_STATE_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8" +BALANCE_TARGET_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da9" # heartbeat_service HEARTBEAT_SVC_UUID = "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0" diff --git a/packages/pid/CMakeLists.txt b/packages/pid/CMakeLists.txt new file mode 100644 index 00000000..abf134d1 --- /dev/null +++ b/packages/pid/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRCS "src/pid.c" + INCLUDE_DIRS "include" +) diff --git a/packages/pid/include/pid/pid.h b/packages/pid/include/pid/pid.h new file mode 100644 index 00000000..83965b7f --- /dev/null +++ b/packages/pid/include/pid/pid.h @@ -0,0 +1,34 @@ +#pragma once + +// Generic discrete PID with clamped integral (anti-windup). +// +// All state lives in the struct so you can run multiple independent loops +// (balance loop, heading loop, etc.) without globals. +// +// Output = Kp*e + Ki*integral + Kd*derivative +// - integral is clamped to ±(output_limit / Ki) so it can never drive +// the I-term past the full output range on its own. +// - output is clamped to ±output_limit. +// - derivative is on error, not measurement — simpler and fine for +// a balance bot where setpoint changes are slow. + +typedef struct { + float kp; + float ki; + float kd; + float integral; + float prev_error; + float output_limit; +} pid_t; + +// Initialize with gains and a symmetric output clamp (±output_limit). +void pid_init(pid_t *p, float kp, float ki, float kd, float output_limit); + +// Compute one tick. Returns clamped output. dt_s: seconds since last call. +float pid_compute(pid_t *p, float error, float dt_s); + +// Zero the integral accumulator. Call on I-dump timer fire. +void pid_reset_integral(pid_t *p); + +// Live gain update without resetting accumulated state. +void pid_set_gains(pid_t *p, float kp, float ki, float kd); diff --git a/packages/pid/src/pid.c b/packages/pid/src/pid.c new file mode 100644 index 00000000..b30cfae6 --- /dev/null +++ b/packages/pid/src/pid.c @@ -0,0 +1,43 @@ +#include "pid/pid.h" + +#include + +static float clampf(float v, float lo, float hi) { + if (v < lo) return lo; + if (v > hi) return hi; + return v; +} + +void pid_init(pid_t *p, float kp, float ki, float kd, float output_limit) { + p->kp = kp; + p->ki = ki; + p->kd = kd; + p->integral = 0.0f; + p->prev_error = 0.0f; + p->output_limit = output_limit; +} + +float pid_compute(pid_t *p, float error, float dt_s) { + // Integrate with anti-windup: clamp so Ki*integral never exceeds output_limit. + p->integral += error * dt_s; + if (p->ki > 1e-9f) { + float ilimit = p->output_limit / p->ki; + p->integral = clampf(p->integral, -ilimit, ilimit); + } + + float derivative = (dt_s > 1e-9f) ? (error - p->prev_error) / dt_s : 0.0f; + p->prev_error = error; + + float out = p->kp * error + p->ki * p->integral + p->kd * derivative; + return clampf(out, -p->output_limit, p->output_limit); +} + +void pid_reset_integral(pid_t *p) { + p->integral = 0.0f; +} + +void pid_set_gains(pid_t *p, float kp, float ki, float kd) { + p->kp = kp; + p->ki = ki; + p->kd = kd; +} diff --git a/packages/sensors/CMakeLists.txt b/packages/sensors/CMakeLists.txt new file mode 100644 index 00000000..67403ee6 --- /dev/null +++ b/packages/sensors/CMakeLists.txt @@ -0,0 +1,5 @@ +idf_component_register( + SRCS "src/imu.c" + INCLUDE_DIRS "include" + PRIV_REQUIRES driver +) diff --git a/packages/sensors/include/sensors/imu.h b/packages/sensors/include/sensors/imu.h new file mode 100644 index 00000000..4561b335 --- /dev/null +++ b/packages/sensors/include/sensors/imu.h @@ -0,0 +1,26 @@ +#pragma once + +#include +#include "driver/i2c.h" +#include "esp_err.h" + +// MPU6050 via I2C. Outputs pitch angle only — this is all a two-wheeled +// balance bot needs. Complementary filter blends gyro integration (fast, +// drifts) with accelerometer geometry (slow, noisy) at a 98/2 ratio tuned +// for a 100 Hz loop. Fusing at higher rates needs less accel weight; lower +// rates need more — keep loop rate and ALPHA in sync if you change timing. +// +// Axis convention (chip mounted flat, Y-axis pointing up when balanced): +// pitch > 0 — robot tilts forward (top toward front) +// pitch < 0 — robot tilts backward +// Flip CONFIG_BALANCE_BOT_IMU_PITCH_INVERT if your mount reverses this. + +esp_err_t imu_init(i2c_port_t port, int sda_pin, int scl_pin, uint8_t addr); + +// Call every control-loop tick. dt_s: elapsed seconds since last call. +// Reads all 14 MPU6050 registers in one I2C transaction and updates the +// internal complementary filter. No-op if imu_init failed. +void imu_update(float dt_s); + +float imu_pitch_deg(void); +bool imu_ready(void); diff --git a/packages/sensors/src/imu.c b/packages/sensors/src/imu.c new file mode 100644 index 00000000..c87220c6 --- /dev/null +++ b/packages/sensors/src/imu.c @@ -0,0 +1,130 @@ +#include "sdkconfig.h" +#if CONFIG_BALANCE_BOT_ENABLED + +#include "sensors/imu.h" + +#include +#include + +#include "driver/i2c.h" +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +static const char *TAG = "imu"; + +// MPU6050 register map (only what we need). +#define REG_PWR_MGMT_1 0x6B +#define REG_ACCEL_XOUT_H 0x3B // start of 14-byte accel+temp+gyro block + +// Full-scale ranges we configure at power-on defaults: +// ±2g → sensitivity 16384 LSB/g +// ±250°/s → sensitivity 131 LSB/(°/s) +#define ACCEL_SCALE 16384.0f +#define GYRO_SCALE 131.0f + +// Complementary filter weight on the gyro side. At 100 Hz: +// 0.98 means accel corrects ~2% of angle each tick → full correction in ~50 ticks. +#define ALPHA 0.98f + +#define I2C_TIMEOUT_MS 10 + +static i2c_port_t s_port; +static uint8_t s_addr; +static float s_pitch = 0.0f; +static bool s_ready = false; + +static esp_err_t mpu_write_reg(uint8_t reg, uint8_t val) { + uint8_t buf[2] = {reg, val}; + return i2c_master_write_to_device(s_port, s_addr, buf, 2, + pdMS_TO_TICKS(I2C_TIMEOUT_MS)); +} + +static esp_err_t mpu_read_regs(uint8_t reg, uint8_t *out, size_t len) { + return i2c_master_write_read_device(s_port, s_addr, ®, 1, + out, len, + pdMS_TO_TICKS(I2C_TIMEOUT_MS)); +} + +esp_err_t imu_init(i2c_port_t port, int sda_pin, int scl_pin, uint8_t addr) { + s_port = port; + s_addr = addr; + + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = sda_pin, + .scl_io_num = scl_pin, + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_pullup_en = GPIO_PULLUP_ENABLE, + .master.clk_speed = 400000, + }; + esp_err_t err = i2c_param_config(port, &conf); + if (err != ESP_OK) { + ESP_LOGE(TAG, "i2c_param_config: %s", esp_err_to_name(err)); + return err; + } + err = i2c_driver_install(port, I2C_MODE_MASTER, 0, 0, 0); + if (err != ESP_OK) { + ESP_LOGE(TAG, "i2c_driver_install: %s", esp_err_to_name(err)); + return err; + } + + // Wake up — MPU6050 powers on in sleep mode. + err = mpu_write_reg(REG_PWR_MGMT_1, 0x00); + if (err != ESP_OK) { + ESP_LOGE(TAG, "MPU6050 not responding at 0x%02x: %s", + addr, esp_err_to_name(err)); + return err; + } + vTaskDelay(pdMS_TO_TICKS(100)); // let sensor stabilize after wakeup + + // Seed the complementary filter with a static accel reading so the + // first few loop ticks don't snap from 0° to the real angle. + uint8_t raw[6]; + if (mpu_read_regs(REG_ACCEL_XOUT_H, raw, 6) == ESP_OK) { + int16_t ax = (int16_t)((raw[0] << 8) | raw[1]); + int16_t az = (int16_t)((raw[4] << 8) | raw[5]); + float ax_g = ax / ACCEL_SCALE; + float az_g = az / ACCEL_SCALE; + s_pitch = atan2f(ax_g, az_g) * (180.0f / (float)M_PI); +#if CONFIG_BALANCE_BOT_IMU_PITCH_INVERT + s_pitch = -s_pitch; +#endif + } + + s_ready = true; + ESP_LOGI(TAG, "MPU6050 ready at 0x%02x — initial pitch %.2f°", addr, s_pitch); + return ESP_OK; +} + +void imu_update(float dt_s) { + if (!s_ready) return; + + // Read accel (bytes 0-5) + temp (bytes 6-7, skipped) + gyro (bytes 8-13) + // in one burst from REG_ACCEL_XOUT_H. + uint8_t raw[14]; + if (mpu_read_regs(REG_ACCEL_XOUT_H, raw, sizeof(raw)) != ESP_OK) return; + + int16_t ax_raw = (int16_t)((raw[0] << 8) | raw[1]); + int16_t az_raw = (int16_t)((raw[4] << 8) | raw[5]); + int16_t gy_raw = (int16_t)((raw[10] << 8) | raw[11]); // pitch-axis gyro (Y) + + float ax_g = ax_raw / ACCEL_SCALE; + float az_g = az_raw / ACCEL_SCALE; + float gy_dps = gy_raw / GYRO_SCALE; // degrees/second + + // Accel-derived angle — stable long-term, noisy short-term. + float accel_pitch = atan2f(ax_g, az_g) * (180.0f / (float)M_PI); + + // Complementary filter: gyro for fast dynamics, accel for drift correction. + s_pitch = ALPHA * (s_pitch + gy_dps * dt_s) + (1.0f - ALPHA) * accel_pitch; + +#if CONFIG_BALANCE_BOT_IMU_PITCH_INVERT + s_pitch = -s_pitch; +#endif +} + +float imu_pitch_deg(void) { return s_pitch; } +bool imu_ready(void) { return s_ready; } + +#endif // CONFIG_BALANCE_BOT_ENABLED diff --git a/protocol/uuids.json b/protocol/uuids.json index 292d5a99..e701b3a3 100644 --- a/protocol/uuids.json +++ b/protocol/uuids.json @@ -20,7 +20,11 @@ "SNAPSHOT_DATA_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da1", "FLASH_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3", "PIN_CONFIG_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4", - "SIGNAL_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5" + "SIGNAL_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5", + "BALANCE_CMD_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6", + "BALANCE_PID_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7", + "BALANCE_STATE_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8", + "BALANCE_TARGET_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da9" }, "heartbeat_service": { "HEARTBEAT_SVC_UUID": "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0", diff --git a/public/ble.js b/public/ble.js index c44e7e9c..2aae3567 100644 --- a/public/ble.js +++ b/public/ble.js @@ -10,6 +10,8 @@ import { CAMERA_SIGNAL_CHAR_UUID, CAMERA_STATUS_CHAR_UUID, OPS_CHAR_UUID, SNAPSHOT_REQUEST_CHAR_UUID, SNAPSHOT_DATA_CHAR_UUID, + BALANCE_CMD_CHAR_UUID, BALANCE_PID_CHAR_UUID, + BALANCE_STATE_CHAR_UUID, BALANCE_TARGET_CHAR_UUID, } from "./uuids.js"; // Chunked-frame protocol shared by OTA + camera signaling: begin carries @@ -27,6 +29,12 @@ export const UUIDS_BY_CAP = { camera: { signal: CAMERA_SIGNAL_CHAR_UUID, status: CAMERA_STATUS_CHAR_UUID }, ops: OPS_CHAR_UUID, snapshot: { request: SNAPSHOT_REQUEST_CHAR_UUID, data: SNAPSHOT_DATA_CHAR_UUID }, + "balance-bot": { + cmd: BALANCE_CMD_CHAR_UUID, + pid: BALANCE_PID_CHAR_UUID, + state: BALANCE_STATE_CHAR_UUID, + target: BALANCE_TARGET_CHAR_UUID, + }, }; export const decodeJson = (dv) => { diff --git a/public/capabilities/runtime/balance-bot.js b/public/capabilities/runtime/balance-bot.js new file mode 100644 index 00000000..066d73ae --- /dev/null +++ b/public/capabilities/runtime/balance-bot.js @@ -0,0 +1,372 @@ +// Schema: { name: "balance-bot", type: "balance-bot" } +// +// Four sections in one card: +// 1. Joypad — writes [lean%, turn%] to BALANCE_CMD_CHAR at ~5 Hz. +// Lean (Y axis) tilts the setpoint; turn (X axis) adds +// a differential directly. No mix() — the firmware mixes. +// 2. PID — P / I / D number inputs. Sends on blur or Enter. +// State line shows live pitch from BALANCE_STATE_CHAR. +// 3. I-dump — dropdown auto-writes on change; no OK button needed. +// 4. Goto — X / Y / θ inputs with Go and Cancel. Phase-1 stub: +// char is wired, firmware stores target but doesn't drive +// to it until CV localization is available (cv branch). +import { UUIDS_BY_CAP, encodeJson, decodeJson } from "../../ble.js"; +import { escapeHtml } from "../../dom.js"; +import { logFor } from "../../log.js"; +import { capSection, isOpen } from "./cap-section.js"; +import { renderEntry } from "./render-bus.js"; + +const CHARS = UUIDS_BY_CAP["balance-bot"]; + +// ── BLE write helpers ──────────────────────────────────────────────────────── + +async function writeCmd(entry, lean, turn) { + const ch = entry.balanceCmdChar; + if (!ch) return; + const v = new Int8Array([lean, turn]); + try { + await ch.writeValueWithoutResponse(new Uint8Array(v.buffer)); + } catch (e) { + logFor(entry, `balance cmd write failed: ${e.message}`); + } +} + +async function writePid(entry) { + const ch = entry.balancePidChar; + if (!ch) return; + const payload = { + p: entry.balancePidP, + i: entry.balancePidI, + d: entry.balancePidD, + idump_s: entry.balanceIdumpS, + }; + try { + await ch.writeValueWithResponse(encodeJson(payload)); + } catch (e) { + logFor(entry, `balance PID write failed: ${e.message}`); + } +} + +async function writeTarget(entry, active) { + const ch = entry.balanceTargetChar; + if (!ch) return; + const payload = { + x: entry.balanceTargetX, + y: entry.balanceTargetY, + theta: entry.balanceTargetTheta, + active, + }; + try { + await ch.writeValueWithResponse(encodeJson(payload)); + } catch (e) { + logFor(entry, `balance target write failed: ${e.message}`); + } +} + +// ── Joypad (no mix — sends raw lean/turn) ─────────────────────────────────── + +function wireBalanceJoypad(entry, pad, knob) { + let activeId = null; + let holdTimer = null; + let lastLean = 0, lastTurn = 0; + let rafId = null; + let pending = null; + + const sendFromXY = (cx, cy) => { + const rect = pad.getBoundingClientRect(); + const radius = rect.width / 2; + const dx = cx - (rect.left + radius); + const dy = cy - (rect.top + radius); + const dist = Math.min(1, Math.hypot(dx, dy) / radius); + const angle = Math.atan2(dy, dx); + const nx = Math.cos(angle) * dist; + const ny = Math.sin(angle) * dist; + knob.style.transform = `translate(${nx * radius}px, ${ny * radius}px)`; + lastLean = Math.max(-100, Math.min(100, Math.round(-ny * 100))); + lastTurn = Math.max(-100, Math.min(100, Math.round(nx * 100))); + writeCmd(entry, lastLean, lastTurn); + }; + + const scheduleUpdate = (x, y) => { + pending = [x, y]; + if (rafId) return; + rafId = requestAnimationFrame(() => { + rafId = null; + if (!pending) return; + const [px, py] = pending; + pending = null; + sendFromXY(px, py); + }); + }; + + const onMove = (e) => { + if (e.pointerId !== activeId) return; + e.preventDefault(); + scheduleUpdate(e.clientX, e.clientY); + }; + + const detach = () => { + window.removeEventListener("pointermove", onMove); + window.removeEventListener("pointerup", onUp); + window.removeEventListener("pointercancel", onUp); + }; + + const clear = () => { + if (activeId === null) return; + activeId = null; + detach(); + if (holdTimer) { clearInterval(holdTimer); holdTimer = null; } + if (rafId) { cancelAnimationFrame(rafId); rafId = null; } + pending = null; + pad.classList.remove("dragging"); + knob.style.transform = ""; + lastLean = lastTurn = 0; + }; + + function onUp(e) { + if (e && e.pointerId !== activeId) return; + clear(); + writeCmd(entry, 0, 0); + } + + pad.addEventListener("pointerdown", (e) => { + if (activeId !== null) return; + e.preventDefault(); + activeId = e.pointerId; + pad.classList.add("dragging"); + sendFromXY(e.clientX, e.clientY); + holdTimer = setInterval(() => writeCmd(entry, lastLean, lastTurn), 200); + window.addEventListener("pointermove", onMove); + window.addEventListener("pointerup", onUp); + window.addEventListener("pointercancel", onUp); + }); + + return { reset: clear }; +} + +// ── Capability factory ─────────────────────────────────────────────────────── + +export function makeBalanceBotCap(schema) { + const name = schema.name || "balance-bot"; + + return { + name, + schema, + + initEntry: () => ({ + balanceCmdChar: null, + balancePidChar: null, + balanceStateChar: null, + balanceTargetChar: null, + balancePitch: null, // live from state notify + balanceSp: null, + balanceMode: null, + balancePidP: 15, + balancePidI: 0.5, + balancePidD: 1.2, + balanceIdumpS: 0, + balanceTargetX: 0, + balanceTargetY: 0, + balanceTargetTheta: 0, + }), + + async probe(entry, service) { + try { + entry.balanceCmdChar = await service.getCharacteristic(CHARS.cmd); + } catch { entry.balanceCmdChar = null; } + + try { + entry.balancePidChar = await service.getCharacteristic(CHARS.pid); + const cur = await entry.balancePidChar.readValue(); + const parsed = decodeJson(cur); + if (parsed) { + if (parsed.p != null) entry.balancePidP = parsed.p; + if (parsed.i != null) entry.balancePidI = parsed.i; + if (parsed.d != null) entry.balancePidD = parsed.d; + if (parsed.idump_s != null) entry.balanceIdumpS = parsed.idump_s; + } + await entry.balancePidChar.startNotifications(); + entry.balancePidChar.addEventListener("characteristicvaluechanged", (e) => { + const p = decodeJson(e.target.value); + if (!p) return; + if (p.p != null) entry.balancePidP = p.p; + if (p.i != null) entry.balancePidI = p.i; + if (p.d != null) entry.balancePidD = p.d; + if (p.idump_s != null) entry.balanceIdumpS = p.idump_s; + patchPidDisplay(entry); + }); + } catch { entry.balancePidChar = null; } + + try { + entry.balanceStateChar = await service.getCharacteristic(CHARS.state); + await entry.balanceStateChar.startNotifications(); + entry.balanceStateChar.addEventListener("characteristicvaluechanged", (e) => { + const s = decodeJson(e.target.value); + if (!s) return; + entry.balancePitch = s.pitch; + entry.balanceSp = s.sp; + entry.balanceMode = s.mode; + patchStateDisplay(entry); + }); + } catch { entry.balanceStateChar = null; } + + try { + entry.balanceTargetChar = await service.getCharacteristic(CHARS.target); + } catch { entry.balanceTargetChar = null; } + }, + + cleanup(entry) { + entry.balanceCmdChar = entry.balancePidChar = + entry.balanceStateChar = entry.balanceTargetChar = null; + }, + + renderSection(entry) { + if (entry.status !== "connected" || !entry.balanceCmdChar) return ""; + + const pitch = entry.balancePitch != null + ? `pitch ${entry.balancePitch.toFixed(1)}°` + : "connecting…"; + const modeStr = entry.balanceMode ? ` · ${entry.balanceMode}` : ""; + const stateText = pitch + modeStr; + + const body = ` +
+
+
+
+
+ + + +
+
+ +
+
+
+
+ + + + + +
+
Goto requires overhead CV localization (cv branch).
+
`; + + const action = ``; + return capSection({ name, label: "Balance Bot", state: stateText, action, body, transport: "ble" }); + }, + + wireActions(entry, node) { + // Joypad + const pad = node.querySelector(".joypad"); + const knob = pad?.querySelector(".joypad-knob"); + let joypad = null; + if (pad && knob) joypad = wireBalanceJoypad(entry, pad, knob); + + // Stop button + node.querySelector("[data-action='balance-stop']")?.addEventListener("click", () => { + joypad?.reset(); + writeCmd(entry, 0, 0); + }); + + // PID inputs — send on blur or Enter + const sendPid = () => writePid(entry); + node.querySelectorAll(".balance-pid-input").forEach(input => { + const key = input.dataset.pid; + input.addEventListener("change", () => { + const v = parseFloat(input.value); + if (!isNaN(v) && v >= 0) { + if (key === "p") entry.balancePidP = v; + if (key === "i") entry.balancePidI = v; + if (key === "d") entry.balancePidD = v; + sendPid(); + } + }); + input.addEventListener("keydown", (e) => { + if (e.key === "Enter") { input.blur(); } + }); + }); + + // I-dump select — immediate write on change, no OK + node.querySelector(".balance-idump-select")?.addEventListener("change", (e) => { + entry.balanceIdumpS = parseInt(e.target.value, 10) || 0; + sendPid(); + }); + + // Goto inputs + node.querySelectorAll(".balance-goto-input").forEach(input => { + input.addEventListener("change", () => { + const k = input.dataset.goto; + const v = parseFloat(input.value); + if (!isNaN(v)) { + if (k === "x") entry.balanceTargetX = v; + if (k === "y") entry.balanceTargetY = v; + if (k === "theta") entry.balanceTargetTheta = v; + } + }); + }); + + node.querySelector("[data-action='balance-go']")?.addEventListener("click", () => { + writeTarget(entry, true); + }); + node.querySelector("[data-action='balance-cancel']")?.addEventListener("click", () => { + writeTarget(entry, false); + }); + }, + }; +} + +// ── Surgical DOM patchers (avoid full re-render on notify) ─────────────────── + +function patchStateDisplay(entry) { + const node = entry.node; + if (!node) return; + const sec = node.querySelector(`.cap-section[data-cap-name="balance-bot"]`); + if (!sec) return; + const stateEl = sec.querySelector(".cap-state"); + if (!stateEl) return; + const pitch = entry.balancePitch != null + ? `pitch ${entry.balancePitch.toFixed(1)}°` + : "connecting…"; + const modeStr = entry.balanceMode ? ` · ${entry.balanceMode}` : ""; + stateEl.textContent = pitch + modeStr; +} + +function patchPidDisplay(entry) { + const node = entry.node; + if (!node) return; + const sec = node.querySelector(`.cap-section[data-cap-name="balance-bot"]`); + if (!sec) return; + // Only patch inputs that are not focused (don't fight the user's typing). + sec.querySelectorAll(".balance-pid-input").forEach(input => { + if (document.activeElement === input) return; + const key = input.dataset.pid; + if (key === "p" && entry.balancePidP != null) input.value = entry.balancePidP; + if (key === "i" && entry.balancePidI != null) input.value = entry.balancePidI; + if (key === "d" && entry.balancePidD != null) input.value = entry.balancePidD; + }); + const sel = sec.querySelector(".balance-idump-select"); + if (sel && document.activeElement !== sel) sel.value = String(entry.balanceIdumpS); +} diff --git a/public/capabilities/runtime/index.js b/public/capabilities/runtime/index.js index d1d45f31..21d3bd73 100644 --- a/public/capabilities/runtime/index.js +++ b/public/capabilities/runtime/index.js @@ -7,6 +7,7 @@ import { makeWifiScanCap } from "./wifi-scan.js"; import { makeWebrtcInstallableCap } from "./webrtc-installable.js"; import { makeMjpegStreamCap } from "./mjpeg-stream.js"; import { makeBleSnapshotCap } from "./ble-snapshot.js"; +import { makeBalanceBotCap } from "./balance-bot.js"; import { setRender as setBusRender } from "./render-bus.js"; export const RUNTIMES = { @@ -18,6 +19,7 @@ export const RUNTIMES = { "webrtc-installable": makeWebrtcInstallableCap, "mjpeg-stream": makeMjpegStreamCap, "ble-snapshot": makeBleSnapshotCap, + "balance-bot": makeBalanceBotCap, }; // All runtime caps share render-bus.js for the back-channel to the diff --git a/public/styles.css b/public/styles.css index 299644e8..fec620a3 100644 --- a/public/styles.css +++ b/public/styles.css @@ -1280,6 +1280,59 @@ details.tray.has-alert > summary::after { -webkit-tap-highlight-color: transparent; } +/* Balance bot — PID tuning + I-dump + goto sections inside the cap-body. */ +.balance-pid-section, +.balance-goto-section { + margin-top: 12px; + padding-top: 10px; + border-top: 1px solid var(--border-default); +} +.balance-pid-row, +.balance-goto-row { + display: flex; + flex-wrap: wrap; + align-items: center; + gap: 8px; +} +.balance-pid-row label, +.balance-goto-row label { + display: flex; + align-items: center; + gap: 4px; + font-size: 13px; + color: var(--ink-muted); +} +.balance-pid-input, +.balance-goto-input { + width: 72px; + padding: 4px 6px; + font: inherit; + font-size: 13px; + border: 1px solid var(--border-default); + border-radius: 6px; + background: var(--bg-elev); + color: var(--ink); + text-align: right; +} +.balance-pid-input:focus, +.balance-goto-input:focus { + outline: 2px solid var(--accent); + outline-offset: 1px; +} +.balance-idump-row { + margin-top: 8px; + font-size: 13px; + color: var(--ink-muted); + display: flex; + align-items: center; + gap: 6px; +} +.balance-goto-section .meta { + margin-top: 6px; + font-size: 12px; + color: var(--ink-muted); +} + /* Segmented control — Apple HIG's recommendation for 2–4 mutually exclusive options. Recessed track inside the surrounding --surface; the active segment rises to --surface with a subtle ring. macOS- diff --git a/public/uuids.js b/public/uuids.js index c3e90c1c..c31674ac 100644 --- a/public/uuids.js +++ b/public/uuids.js @@ -1,4 +1,4 @@ -// Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. +// Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. // Run `make gen-uuids` (or `python3 tools/gen-uuids.py`) after editing the JSON. // main_service @@ -22,6 +22,10 @@ export const SNAPSHOT_DATA_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da1"; export const FLASH_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3"; export const PIN_CONFIG_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4"; export const SIGNAL_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5"; +export const BALANCE_CMD_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6"; +export const BALANCE_PID_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7"; +export const BALANCE_STATE_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8"; +export const BALANCE_TARGET_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da9"; // heartbeat_service export const HEARTBEAT_SVC_UUID = "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0"; From 2db46268abd1d415f88964af8937df26b4794326 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 21:44:03 -0400 Subject: [PATCH 2/4] Docs: document packages/ layout and balance-bot firmware files Co-Authored-By: Claude Sonnet 4.6 --- README.md | 1 + firmware/esp32_robot_idf/README.md | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/README.md b/README.md index f3375bc0..3f4b3b24 100644 --- a/README.md +++ b/README.md @@ -90,6 +90,7 @@ Commit + push when ready. CI rebuilds firmware artifacts on every `firmware/**` - `firmware/esp32_robot_idf/` — ESP32 firmware (ESP-IDF; LED, WiFi onboarding, OTA, motors, camera, WebRTC peer). - `firmware/pi_robot/` — Raspberry Pi firmware (Python + `bless`). Same service UUID and characteristic UUIDs as ESP32. [Details](firmware/pi_robot/README.md). +- `packages/` — reusable ESP-IDF components. `pid/` (generic discrete PID), `sensors/` (MPU6050 driver). Added to `EXTRA_COMPONENT_DIRS` in the ESP32 firmware build. - `public/` — the dashboard (static ES modules, no build step). `docs/` is a symlink for GitHub Pages. - `tests/` — pure-function unit tests; `make smoke`. Manual checklist in [SMOKE.md](SMOKE.md). - `.claude/` — agent + project context (wedge, model discipline, control-loop architecture). diff --git a/firmware/esp32_robot_idf/README.md b/firmware/esp32_robot_idf/README.md index 200dc611..b96c5b4c 100644 --- a/firmware/esp32_robot_idf/README.md +++ b/firmware/esp32_robot_idf/README.md @@ -34,6 +34,12 @@ main/ telemetry.{c,h} — uptime / heap / IP, every 10s webrtc_peer.{c,h} — wss signaling, esp_peer, ota + video + control data channels restart_util.{c,h} — deferred restart (used by pin/cam/ota commit paths) + balance.{c,h} — 100 Hz FreeRTOS balance task (core 1); PID + lean/turn + I-dump timer + Kconfig.projbuild — CONFIG_BALANCE_BOT_ENABLED + I2C pin / address / invert flags + +packages/ (EXTRA_COMPONENT_DIRS) + pid/ — generic discrete PID with anti-windup (no platform deps) + sensors/ — MPU6050 I2C driver, complementary filter → pitch angle ``` ## Partition table From bbb54ecc5507753c7a092be4450d72dabd657361 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 21:56:51 -0400 Subject: [PATCH 3/4] packages/kalman: 2-state C++ Kalman filter for angle + gyro-bias - AngleFilter class (kalman.hpp): predict/update with online bias estimation, drop-in upgrade over complementary filter with same inputs (gyro rate + accel angle) - C wrapper API (kalman.h): kalman_angle_create/update/get/reset/destroy for use from .c files - Registered in firmware EXTRA_COMPONENT_DIRS; add REQUIRES kalman to any component that needs it Co-Authored-By: Claude Sonnet 4.6 --- firmware/esp32_robot_idf/CMakeLists.txt | 3 +- packages/kalman/CMakeLists.txt | 4 ++ packages/kalman/include/kalman/kalman.h | 21 +++++++ packages/kalman/include/kalman/kalman.hpp | 43 +++++++++++++ packages/kalman/src/kalman.cpp | 75 +++++++++++++++++++++++ 5 files changed, 145 insertions(+), 1 deletion(-) create mode 100644 packages/kalman/CMakeLists.txt create mode 100644 packages/kalman/include/kalman/kalman.h create mode 100644 packages/kalman/include/kalman/kalman.hpp create mode 100644 packages/kalman/src/kalman.cpp diff --git a/firmware/esp32_robot_idf/CMakeLists.txt b/firmware/esp32_robot_idf/CMakeLists.txt index 19c2d4f0..df14604c 100644 --- a/firmware/esp32_robot_idf/CMakeLists.txt +++ b/firmware/esp32_robot_idf/CMakeLists.txt @@ -36,7 +36,8 @@ endif() list(APPEND EXTRA_COMPONENT_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/pid" - "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/sensors") + "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/sensors" + "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/kalman") include($ENV{IDF_PATH}/tools/cmake/project.cmake) diff --git a/packages/kalman/CMakeLists.txt b/packages/kalman/CMakeLists.txt new file mode 100644 index 00000000..c2873c66 --- /dev/null +++ b/packages/kalman/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRCS "src/kalman.cpp" + INCLUDE_DIRS "include" +) diff --git a/packages/kalman/include/kalman/kalman.h b/packages/kalman/include/kalman/kalman.h new file mode 100644 index 00000000..35434027 --- /dev/null +++ b/packages/kalman/include/kalman/kalman.h @@ -0,0 +1,21 @@ +#pragma once + +// C-compatible API for the Kalman angle filter. +// Use this header from .c files; use kalman.hpp directly from .cpp files. + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kalman_angle_s kalman_angle_t; + +kalman_angle_t *kalman_angle_create(float q_angle, float q_bias, float r_measure); +float kalman_angle_update(kalman_angle_t *k, float gyro_rate_dps, + float accel_angle_deg, float dt_s); +float kalman_angle_get(const kalman_angle_t *k); +void kalman_angle_reset(kalman_angle_t *k, float initial_angle); +void kalman_angle_destroy(kalman_angle_t *k); + +#ifdef __cplusplus +} +#endif diff --git a/packages/kalman/include/kalman/kalman.hpp b/packages/kalman/include/kalman/kalman.hpp new file mode 100644 index 00000000..48ca940d --- /dev/null +++ b/packages/kalman/include/kalman/kalman.hpp @@ -0,0 +1,43 @@ +#pragma once + +// 2-state discrete Kalman filter for IMU angle + gyro-bias estimation. +// Drop-in upgrade over a complementary filter: same inputs, less phase lag, +// online gyro-bias correction. +// +// State: x = [angle, gyro_bias] +// Input: gyro rate (deg/s) — corrected internally for bias +// Meas: accel-derived angle (deg) +// +// Noise tuning guide: +// q_angle — how fast true angle can change unexpectedly (smaller → smoother) +// q_bias — how fast gyro bias drifts (very small, bias is near-constant) +// r_measure — accel noise; increase if your accel is noisy or bot vibrates + +namespace kalman { + +struct AngleConfig { + float q_angle = 0.001f; + float q_bias = 0.003f; + float r_measure = 0.03f; +}; + +class AngleFilter { +public: + explicit AngleFilter(AngleConfig cfg = {}); + + // Feed one sample. Returns filtered angle in degrees. + float update(float gyro_rate_dps, float accel_angle_deg, float dt_s); + + float angle() const { return angle_; } + float bias() const { return bias_; } + + void reset(float initial_angle = 0.0f); + +private: + AngleConfig cfg_; + float angle_; + float bias_; + float P_[2][2]; // error covariance matrix +}; + +} // namespace kalman diff --git a/packages/kalman/src/kalman.cpp b/packages/kalman/src/kalman.cpp new file mode 100644 index 00000000..36878974 --- /dev/null +++ b/packages/kalman/src/kalman.cpp @@ -0,0 +1,75 @@ +#include "kalman/kalman.hpp" +#include "kalman/kalman.h" + +namespace kalman { + +AngleFilter::AngleFilter(AngleConfig cfg) + : cfg_(cfg), angle_(0.0f), bias_(0.0f) +{ + P_[0][0] = 0.0f; P_[0][1] = 0.0f; + P_[1][0] = 0.0f; P_[1][1] = 0.0f; +} + +float AngleFilter::update(float gyro_rate_dps, float accel_angle_deg, float dt_s) { + // Predict step — integrate gyro (bias-corrected) and propagate covariance. + angle_ += dt_s * (gyro_rate_dps - bias_); + P_[0][0] += dt_s * (dt_s * P_[1][1] - P_[0][1] - P_[1][0] + cfg_.q_angle); + P_[0][1] -= dt_s * P_[1][1]; + P_[1][0] -= dt_s * P_[1][1]; + P_[1][1] += cfg_.q_bias * dt_s; + + // Update step — H = [1, 0]: accel measures angle directly. + float S = P_[0][0] + cfg_.r_measure; + float K0 = P_[0][0] / S; + float K1 = P_[1][0] / S; + + float y = accel_angle_deg - angle_; + angle_ += K0 * y; + bias_ += K1 * y; + + float P00 = P_[0][0]; + float P01 = P_[0][1]; + P_[0][0] -= K0 * P00; + P_[0][1] -= K0 * P01; + P_[1][0] -= K1 * P00; + P_[1][1] -= K1 * P01; + + return angle_; +} + +void AngleFilter::reset(float initial_angle) { + angle_ = initial_angle; + bias_ = 0.0f; + P_[0][0] = 0.0f; P_[0][1] = 0.0f; + P_[1][0] = 0.0f; P_[1][1] = 0.0f; +} + +} // namespace kalman + +// ── C wrappers ──────────────────────────────────────────────────────────── + +struct kalman_angle_s : public kalman::AngleFilter { + kalman_angle_s(float q_angle, float q_bias, float r_measure) + : kalman::AngleFilter({q_angle, q_bias, r_measure}) {} +}; + +kalman_angle_t *kalman_angle_create(float q_angle, float q_bias, float r_measure) { + return new kalman_angle_s(q_angle, q_bias, r_measure); +} + +float kalman_angle_update(kalman_angle_t *k, float gyro_rate_dps, + float accel_angle_deg, float dt_s) { + return k->update(gyro_rate_dps, accel_angle_deg, dt_s); +} + +float kalman_angle_get(const kalman_angle_t *k) { + return k->angle(); +} + +void kalman_angle_reset(kalman_angle_t *k, float initial_angle) { + k->reset(initial_angle); +} + +void kalman_angle_destroy(kalman_angle_t *k) { + delete k; +} From bafb8276458fe2fd5203637b2565679db0199e47 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 22:05:07 -0400 Subject: [PATCH 4/4] IMU filter: extract complementary filter as package, add Kalman option - packages/complementary: standalone comp_filter_t with init/update/angle API - packages/sensors/Kconfig: BALANCE_BOT_IMU_FILTER choice (complementary default); alpha x100 for complementary; Q_angle/Q_bias/R_measure x1000 for Kalman - imu.c: delegates filtering to whichever package Kconfig selects; both packages always linked, dead code eliminated at link time - firmware CMakeLists: adds packages/complementary to EXTRA_COMPONENT_DIRS Select filter via: idf.py menuconfig -> IMU filter Co-Authored-By: Claude Sonnet 4.6 --- firmware/esp32_robot_idf/CMakeLists.txt | 1 + packages/complementary/CMakeLists.txt | 4 ++ .../include/complementary/complementary.h | 23 +++++++ packages/complementary/src/complementary.c | 17 +++++ packages/sensors/CMakeLists.txt | 2 +- packages/sensors/Kconfig | 57 ++++++++++++++++ packages/sensors/include/sensors/imu.h | 7 +- packages/sensors/src/imu.c | 66 +++++++++++++------ 8 files changed, 152 insertions(+), 25 deletions(-) create mode 100644 packages/complementary/CMakeLists.txt create mode 100644 packages/complementary/include/complementary/complementary.h create mode 100644 packages/complementary/src/complementary.c create mode 100644 packages/sensors/Kconfig diff --git a/firmware/esp32_robot_idf/CMakeLists.txt b/firmware/esp32_robot_idf/CMakeLists.txt index df14604c..94203476 100644 --- a/firmware/esp32_robot_idf/CMakeLists.txt +++ b/firmware/esp32_robot_idf/CMakeLists.txt @@ -37,6 +37,7 @@ endif() list(APPEND EXTRA_COMPONENT_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/pid" "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/sensors" + "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/complementary" "${CMAKE_CURRENT_SOURCE_DIR}/../../packages/kalman") include($ENV{IDF_PATH}/tools/cmake/project.cmake) diff --git a/packages/complementary/CMakeLists.txt b/packages/complementary/CMakeLists.txt new file mode 100644 index 00000000..92c5c8fd --- /dev/null +++ b/packages/complementary/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRCS "src/complementary.c" + INCLUDE_DIRS "include" +) diff --git a/packages/complementary/include/complementary/complementary.h b/packages/complementary/include/complementary/complementary.h new file mode 100644 index 00000000..f082c76e --- /dev/null +++ b/packages/complementary/include/complementary/complementary.h @@ -0,0 +1,23 @@ +#pragma once + +// Discrete complementary filter for IMU pitch estimation. +// Blends gyro integration (fast, drifts long-term) with accel-derived angle +// (slow, noisy short-term) using a fixed trust ratio alpha. +// +// Typical alpha at 100 Hz: 0.98 — accel corrects ~2% per tick (~50 ticks to +// full correction). Lower loop rates need a smaller alpha to avoid drift. +// Raise alpha to trust the gyro more; lower it to follow the accel faster. + +typedef struct { + float alpha; + float angle; +} comp_filter_t; + +// Initialize with trust ratio and a known starting angle (degrees). +void comp_filter_init(comp_filter_t *f, float alpha, float initial_angle_deg); + +// Feed one sample. Returns filtered angle in degrees. +float comp_filter_update(comp_filter_t *f, float gyro_rate_dps, + float accel_angle_deg, float dt_s); + +float comp_filter_angle(const comp_filter_t *f); diff --git a/packages/complementary/src/complementary.c b/packages/complementary/src/complementary.c new file mode 100644 index 00000000..73c2c6e3 --- /dev/null +++ b/packages/complementary/src/complementary.c @@ -0,0 +1,17 @@ +#include "complementary/complementary.h" + +void comp_filter_init(comp_filter_t *f, float alpha, float initial_angle_deg) { + f->alpha = alpha; + f->angle = initial_angle_deg; +} + +float comp_filter_update(comp_filter_t *f, float gyro_rate_dps, + float accel_angle_deg, float dt_s) { + f->angle = f->alpha * (f->angle + gyro_rate_dps * dt_s) + + (1.0f - f->alpha) * accel_angle_deg; + return f->angle; +} + +float comp_filter_angle(const comp_filter_t *f) { + return f->angle; +} diff --git a/packages/sensors/CMakeLists.txt b/packages/sensors/CMakeLists.txt index 67403ee6..1bee01a3 100644 --- a/packages/sensors/CMakeLists.txt +++ b/packages/sensors/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS "src/imu.c" INCLUDE_DIRS "include" - PRIV_REQUIRES driver + PRIV_REQUIRES driver complementary kalman ) diff --git a/packages/sensors/Kconfig b/packages/sensors/Kconfig new file mode 100644 index 00000000..ea505950 --- /dev/null +++ b/packages/sensors/Kconfig @@ -0,0 +1,57 @@ +menu "IMU filter" + depends on BALANCE_BOT_ENABLED + + choice BALANCE_BOT_IMU_FILTER + prompt "Pitch filter" + default BALANCE_BOT_IMU_FILTER_COMPLEMENTARY + + config BALANCE_BOT_IMU_FILTER_COMPLEMENTARY + bool "Complementary filter" + help + Fixed-ratio blend of gyro integration and accel-derived angle. + Simple, zero-startup-cost, no tuning required. + + config BALANCE_BOT_IMU_FILTER_KALMAN + bool "Kalman filter" + help + 2-state discrete Kalman filter (angle + gyro-bias). + Lower phase lag and online bias correction vs complementary. + Requires tuning Q and R to match your hardware noise. + endchoice + + config BALANCE_BOT_IMU_COMPLEMENTARY_ALPHA_PCT + int "Alpha (x100, e.g. 98 = 0.98)" + default 98 + range 90 99 + depends on BALANCE_BOT_IMU_FILTER_COMPLEMENTARY + help + Gyro trust ratio. At 100 Hz, 98 means accel corrects ~2% per tick. + Lower values follow the accel faster but pass more accel noise. + + config BALANCE_BOT_IMU_KALMAN_Q_ANGLE_MILLI + int "Q_angle (x1000, e.g. 1 = 0.001)" + default 1 + range 1 1000 + depends on BALANCE_BOT_IMU_FILTER_KALMAN + help + Process noise for angle. Lower = smoother but slower to track + true angle changes. + + config BALANCE_BOT_IMU_KALMAN_Q_BIAS_MILLI + int "Q_bias (x1000, e.g. 3 = 0.003)" + default 3 + range 1 100 + depends on BALANCE_BOT_IMU_FILTER_KALMAN + help + Process noise for gyro bias. Keep small — bias drifts slowly. + + config BALANCE_BOT_IMU_KALMAN_R_MEASURE_MILLI + int "R_measure (x1000, e.g. 30 = 0.030)" + default 30 + range 1 1000 + depends on BALANCE_BOT_IMU_FILTER_KALMAN + help + Measurement noise variance for accel angle. Increase if your + accel is noisy or the bot vibrates significantly. + +endmenu diff --git a/packages/sensors/include/sensors/imu.h b/packages/sensors/include/sensors/imu.h index 4561b335..bc3f960f 100644 --- a/packages/sensors/include/sensors/imu.h +++ b/packages/sensors/include/sensors/imu.h @@ -5,10 +5,9 @@ #include "esp_err.h" // MPU6050 via I2C. Outputs pitch angle only — this is all a two-wheeled -// balance bot needs. Complementary filter blends gyro integration (fast, -// drifts) with accelerometer geometry (slow, noisy) at a 98/2 ratio tuned -// for a 100 Hz loop. Fusing at higher rates needs less accel weight; lower -// rates need more — keep loop rate and ALPHA in sync if you change timing. +// balance bot needs. Filter is selected at build time via menuconfig: +// BALANCE_BOT_IMU_FILTER_COMPLEMENTARY — fixed-ratio blend (default) +// BALANCE_BOT_IMU_FILTER_KALMAN — 2-state Kalman with bias correction // // Axis convention (chip mounted flat, Y-axis pointing up when balanced): // pitch > 0 — robot tilts forward (top toward front) diff --git a/packages/sensors/src/imu.c b/packages/sensors/src/imu.c index c87220c6..fd89cbda 100644 --- a/packages/sensors/src/imu.c +++ b/packages/sensors/src/imu.c @@ -11,6 +11,12 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" +#if CONFIG_BALANCE_BOT_IMU_FILTER_COMPLEMENTARY +#include "complementary/complementary.h" +#else +#include "kalman/kalman.h" +#endif + static const char *TAG = "imu"; // MPU6050 register map (only what we need). @@ -23,10 +29,6 @@ static const char *TAG = "imu"; #define ACCEL_SCALE 16384.0f #define GYRO_SCALE 131.0f -// Complementary filter weight on the gyro side. At 100 Hz: -// 0.98 means accel corrects ~2% of angle each tick → full correction in ~50 ticks. -#define ALPHA 0.98f - #define I2C_TIMEOUT_MS 10 static i2c_port_t s_port; @@ -34,6 +36,12 @@ static uint8_t s_addr; static float s_pitch = 0.0f; static bool s_ready = false; +#if CONFIG_BALANCE_BOT_IMU_FILTER_COMPLEMENTARY +static comp_filter_t s_filter; +#else +static kalman_angle_t *s_filter; +#endif + static esp_err_t mpu_write_reg(uint8_t reg, uint8_t val) { uint8_t buf[2] = {reg, val}; return i2c_master_write_to_device(s_port, s_addr, buf, 2, @@ -78,22 +86,39 @@ esp_err_t imu_init(i2c_port_t port, int sda_pin, int scl_pin, uint8_t addr) { } vTaskDelay(pdMS_TO_TICKS(100)); // let sensor stabilize after wakeup - // Seed the complementary filter with a static accel reading so the - // first few loop ticks don't snap from 0° to the real angle. + // Seed the filter with a static accel reading so the first few loop + // ticks don't snap from 0° to the real angle. + float initial_pitch = 0.0f; uint8_t raw[6]; if (mpu_read_regs(REG_ACCEL_XOUT_H, raw, 6) == ESP_OK) { int16_t ax = (int16_t)((raw[0] << 8) | raw[1]); int16_t az = (int16_t)((raw[4] << 8) | raw[5]); - float ax_g = ax / ACCEL_SCALE; - float az_g = az / ACCEL_SCALE; - s_pitch = atan2f(ax_g, az_g) * (180.0f / (float)M_PI); -#if CONFIG_BALANCE_BOT_IMU_PITCH_INVERT - s_pitch = -s_pitch; -#endif + initial_pitch = atan2f(ax / ACCEL_SCALE, az / ACCEL_SCALE) + * (180.0f / (float)M_PI); } +#if CONFIG_BALANCE_BOT_IMU_FILTER_COMPLEMENTARY + comp_filter_init(&s_filter, + CONFIG_BALANCE_BOT_IMU_COMPLEMENTARY_ALPHA_PCT / 100.0f, + initial_pitch); +#else + s_filter = kalman_angle_create( + CONFIG_BALANCE_BOT_IMU_KALMAN_Q_ANGLE_MILLI / 1000.0f, + CONFIG_BALANCE_BOT_IMU_KALMAN_Q_BIAS_MILLI / 1000.0f, + CONFIG_BALANCE_BOT_IMU_KALMAN_R_MEASURE_MILLI / 1000.0f); + kalman_angle_reset(s_filter, initial_pitch); +#endif + + s_pitch = initial_pitch; s_ready = true; - ESP_LOGI(TAG, "MPU6050 ready at 0x%02x — initial pitch %.2f°", addr, s_pitch); + ESP_LOGI(TAG, "MPU6050 ready at 0x%02x — filter: %s, initial pitch %.2f°", + addr, +#if CONFIG_BALANCE_BOT_IMU_FILTER_COMPLEMENTARY + "complementary", +#else + "kalman", +#endif + s_pitch); return ESP_OK; } @@ -109,15 +134,16 @@ void imu_update(float dt_s) { int16_t az_raw = (int16_t)((raw[4] << 8) | raw[5]); int16_t gy_raw = (int16_t)((raw[10] << 8) | raw[11]); // pitch-axis gyro (Y) - float ax_g = ax_raw / ACCEL_SCALE; - float az_g = az_raw / ACCEL_SCALE; - float gy_dps = gy_raw / GYRO_SCALE; // degrees/second - - // Accel-derived angle — stable long-term, noisy short-term. + float ax_g = ax_raw / ACCEL_SCALE; + float az_g = az_raw / ACCEL_SCALE; + float gy_dps = gy_raw / GYRO_SCALE; float accel_pitch = atan2f(ax_g, az_g) * (180.0f / (float)M_PI); - // Complementary filter: gyro for fast dynamics, accel for drift correction. - s_pitch = ALPHA * (s_pitch + gy_dps * dt_s) + (1.0f - ALPHA) * accel_pitch; +#if CONFIG_BALANCE_BOT_IMU_FILTER_COMPLEMENTARY + s_pitch = comp_filter_update(&s_filter, gy_dps, accel_pitch, dt_s); +#else + s_pitch = kalman_angle_update(s_filter, gy_dps, accel_pitch, dt_s); +#endif #if CONFIG_BALANCE_BOT_IMU_PITCH_INVERT s_pitch = -s_pitch;