PID-balance-robot

Imported from yixiaosz/PID-balance-robot (AGENTS.md). Install upstream with npx skills add yixiaosz/PID-balance-robot. Copyright stays with the author.

AGENTS.md — RoboMaster 两轮自平衡车（PID 学习项目）

本文档是项目的唯一权威规格说明。AI agent 在开始任何编码任务前必须先完整阅读本文档。 项目所有者的核心目标是学习 PID 调参，硬件和底层驱动只是手段——优先保证控制回路代码清晰、参数可调、数据可视化，不要过度工程化。

1. 项目目标

用 RoboMaster 套件从零搭建一台两轮自平衡小车，分阶段实现：

1. CAN 总线控制两个 M2006 电机（里程碑 2，见 §6）
2. 读取板载 IMU 并融合输出 pitch 角（里程碑 3）
3. 直立环 PD 控制，车能站住不倒（里程碑 4）
4. 速度环 PI，车能原地停稳、受推后回位（里程碑 5）
5. 转向差速控制（里程碑 6）

每个里程碑都必须可独立验证后才进入下一个。

2. 硬件清单与接线

2.1 物料

2.2 电气接线

6S电池(24V) → XT60 → 电调中心板（总开关/保险）
                          ├── XT30 → C610 #1（CAN ID=1）→ M2006 左轮
                          ├── XT30 → C610 #2（CAN ID=2）→ M2006 右轮
                          └── 24V → DC-DC → 12V → 开发板A型 DC座(5.5×2.1)

CAN 总线（菊花链，双绞线 CANH/CANL/GND）：
  开发板 CAN1(PD1/PD0) ── C610#1 ── C610#2
  总线首尾两端各并 120Ω 终端电阻（C610 是否板载终端电阻待实测确认，缺则外加）

调试与 telemetry：
  ST-Link → SWDIO(PA13)/SWCLK(PA14)/GND → 开发板 SWD 口
  USB-TTL RX → PG14(USART6_TX)  或 A板USB虚拟串口（调 PID 用）

ST-Link 单独供电测试：开发板 SWD 口白色 4-pin 插座为 3.3V / GND / SWDIO(PA13) / SWCLK(PA14)（非标准 Dupont）。在仅验证烧录与主循环时，可只用 ST-Link 的 3.3V 与 GND 给板子供电，无需接 12V 电池或 DC-DC。实测 STM32F427 + HAL 基础运行可从 ST-Link 3.3V 取电；但电机、CAN 总线满载时必须使用 12V 供电。

2.3 左右轮安装方向

两个电机镜像安装，同一符号的电流指令下两轮转向相反。软件中必须定义 WHEEL_DIR_LEFT / WHEEL_DIR_RIGHT 符号常量统一处理，不要散落硬编码。

当前代码定义（src/board_pins.h）：

#define WHEEL_DIR_LEFT  (+1)
#define WHEEL_DIR_RIGHT (-1)

这两个符号必须在里程碑 2 的架空测试中验证并修正。

3. 开发环境

• OS：Ubuntu 26.04
• IDE：VSCode + PlatformIO IDE 扩展
• 框架：stm32cube（STM32 HAL 库，与 DJI 官方例程同一底层，方便移植官方代码）
• 烧录/调试：OpenOCD via ST-Link，upload_protocol = stlink
• 波形上位机：VOFA+（Linux 版），数据用 printf 逗号分隔格式发送

3.1 platformio.ini（已验证可用）

[env:robomaster_a]
platform = ststm32
board = robomaster_a          ; 自定义 board JSON，见 ./boards/robomaster_a.json
framework = stm32cube
upload_protocol = stlink
debug_tool = stlink
monitor_speed = 115200
monitor_filters = direct
build_flags =
    -O2
    -D HSE_VALUE=12000000U      ; A板外接 12MHz 晶振，已由官方例程时钟配置验证
    -D USE_HAL_DRIVER
    -Wall
    -Wextra
    -Wno-unused-parameter

自定义 board JSON（boards/robomaster_a.json）指定了 STM32F427IIH6、180 MHz、2 MB Flash、256 KB RAM、ST-Link 上传/调试。pio boards | grep robomaster 应能识别。

3.2 官方参考资源（只读参考，代码逻辑可移植）

• DJI 官方例程（Keil 工程，逻辑可移植）：./robomaster-devboard-examples/
  • Imu/ 例程提供了 SPI5 + USART6 的引脚与时钟配置，本项目 HAL 初始化直接参考并扩展了该例程。
• RoboMaster 开发板 A 型 PDF（所有引脚分配以此为准): ./'RoboMaster 开发板A型使用说明.pdf'

3.3 已验证的引脚定义

以下引脚已根据 A 板 PDF 原理图和官方 Imu 例程验证，并集中在 src/board_pins.h 中定义。

注意：src/board_pins.h 是本项目唯一的引脚来源。如需更换 telemetry 到 USART1（PB6/PB7）或 CAN 到 CAN2（PB12/PB13），必须同步修改 src/hal_init.cpp 和 src/main.h 中的外设句柄声明。

4. 关键协议事实（CAN 电机控制，已确认可直接使用）

新手三大坑（写代码时检查）：

1. 忘记配置 CAN 接收过滤器（应配置掩码全 0 放行）和 HAL_CAN_Start() → 收不到反馈
2. 控制帧字节序弄反（必须高字节在前）
3. 发送频率过低 → 电调触发超时保护停转（必须 ≥ 100Hz，本项目固定 1kHz）

5. 软件架构

5.1 目录结构

├── boards/
│   └── robomaster_a.json   # PlatformIO 自定义 board 定义
├── platformio.ini
├── src/
│   ├── main.cpp            # 初始化 + 主循环（只做调度，不放控制逻辑）
│   ├── main.h              # HAL 句柄与初始化函数声明
│   ├── hal_init.cpp        # 手动实现的 HAL 外设初始化（时钟/GPIO/CAN/SPI/UART/TIM）
│   ├── board_pins.h        # 所有引脚定义（已验证，唯一引脚来源）
│   ├── stm32f4xx_it.cpp    # 中断向量处理（含 SysTick_Handler，HAL 时间基准必需）
│   ├── can_motor.cpp/.h    # CAN 收发 + C610 协议封装（发送电流指令、解析反馈）
│   ├── imu.cpp/.h          # MPU6500 SPI 驱动 + 姿态融合
│   ├── control.cpp/.h      # 控制任务（1kHz）：串级 PID 主逻辑
│   ├── pid.cpp/.h          # 通用 PID 结构体（限幅、积分抗饱和）
│   ├── telemetry.cpp/.h    # 串口波形输出（VOFA+ 格式）
│   └── safety.cpp/.h       # 安全保护（见 §7）
└── lib/                    # 移植自官方例程的驱动代码放这里

本项目不强制依赖 STM32CubeMX。src/hal_init.cpp 已包含完整的外设初始化与时钟配置，基于官方 Imu 例程并扩展了 CAN1 与 TIM2。若后续需要添加新外设，可继续在该文件中补充，或改用 CubeMX 生成后替换。

5.2 控制回路拓扑（串级 PID）

速度环 PI(轮速rpm)          直立环 PD
目标速度 ──►[PI]──► 目标倾角偏置 ─►(+)──►[PD]──► 电流指令 ─► CAN 0x200
             ▲                    ▲      ▲
          编码器转速           pitch角  角速度gyro
          (反馈帧)             (IMU融合) (IMU)
转向：左右轮差速偏置叠加在直立环输出之后

• 控制频率：1 kHz（TIM2 定时器中断，抖动必须小）
• 姿态融合：先用互补滤波（pitch = 0.98*(pitch + gyro*dt) + 0.02*acc_pitch），起步够用；稳定后可升级 Mahony
• IMU 零偏标定：上电静止 1 秒采 500 组陀螺仪数据取均值，之后每次减去。必须实现，否则 pitch 漂移
• 所有 PID 增益集中定义在 control.h 顶部，禁止散落在代码中——这是本项目的核心学习目标

5.3 遥测输出（调 PID 的生命线，里程碑 1 就要搭好）

每 5ms 通过串口输出一行 CSV（VOFA+ FireWater 协议）：

pitch, gyro_y, target_current, actual_current, rpm_left, rpm_right

默认使用 USART6 @ 115200 baud（PG14 TX / PG9 RX）。连接 VOFA+ 时选择对应串口，协议选 FireWater（逗号分隔 + \r\n）。

注意：A 板 micro-USB 为 USB OTG 口，本固件未实现 USB CDC 虚拟串口；调参时必须使用外部 USB-TTL 适配器（RX 接 PG14，GND 共地）。

6. 里程碑验收标准

7. 安全约束（硬性要求，任何代码变更不得违反）

1. 倾倒断电：|pitch| > 30° 立即输出零电流并锁存故障状态，需人工复位
2. 首次通电测试一律轮子架空，确认转向和符号正确后再落地
3. 电流指令软限幅：直立环调试阶段限 ±3000，确认稳定后再放宽
4. 看门狗：控制任务 10ms 未执行则电机电流清零
5. M2006 扭矩大，失控甩动危险——调试时人车保持距离，紧急情况下直接拔电池

8. 编码规范

• C++，但保持嵌入式朴素风格：不用 RTTI/异常，慎用堆分配（启动期分配除外）
• 每个外设驱动一个模块，头文件写清接口注释（输入/输出/单位/频率）
• 单位一律 SI + 显式后缀命名：pitch_deg、gyro_dps、current_raw、rpm
• 魔法数字集中在 board_pins.h / control.h
• 移植官方例程代码时保留原作者注释，并标注来源文件

9. 常用命令

# 编译
~/.platformio/penv/bin/pio run

# 上传固件（ST-Link）
~/.platformio/penv/bin/pio run --target upload

# 串口监视器（VOFA+ 也适用）
~/.platformio/penv/bin/pio device monitor

# 清理构建
~/.platformio/penv/bin/pio run --target clean

11. 调试记录：首次 ST-Link 烧录与 LED 验证

本节记录 2026-08-15 首次使用 ST-Link 烧录时的排查过程，便于后续复现与维护。

11.1 环境

• 供电：ST-Link 3.3V（未接 12V 电池/DC-DC）
• 调试器：ST-Link V2（OpenOCD via PlatformIO）
• 目标板：RoboMaster 开发板 A 型
• 已连接：SWDIO/SWCLK/GND/3.3V（白色 4-pin 插座）
• 未连接：USB-TTL 串口线、电机、电调

11.2 现象

• pio run -t upload 成功：Program/Verify/Reset 均 OK。
• 但 PE11 心跳 LED 不闪烁，pio device monitor 只列出 /dev/ttyS*（无 USB 串口设备）。

11.3 排查过程

1. 排除硬件/电源：写了一个无 HAL 的裸机 GPIO 翻转程序，PE11 正常闪烁 → 硬件、ST-Link 供电、引脚映射都正确。
2. 定位到 HAL_Init()：加入 HAL_Init() 后程序停止翻转；进一步测试确认 HAL_Init() 未返回。
3. 定位到 SysTick：覆盖弱符号 HAL_InitTick() 为空函数后，HAL_Init() 可返回，LED 恢复闪烁。
4. 根因：stm32cube 框架的 GCC 启动文件中 SysTick_Handler 为弱定义，默认指向 Default_Handler（死循环）。HAL_Init() 使能 SysTick 中断后，首次 tick 即进入死循环，主循环永远得不到执行。
5. 修复：新增 src/stm32f4xx_it.cpp，提供 SysTick_Handler 并调用 HAL_IncTick()。

11.4 关键结论

• 必须文件：src/stm32f4xx_it.cpp 中的 SysTick_Handler 是 HAL 时间基准的必需品，不可删除。
• ST-Link 供电可行：仅验证烧录、LED、主循环时，ST-Link 3.3V 足以驱动板子；12V 仅在接电机/CAN 总线负载时才需要。
• HSE 可启动：在 ST-Link 供电下，原始 SystemClock_Config() 使用外部 12MHz HSE 未出现超时，时钟配置成功。
• 串口需要 USB-TTL 适配器：A 板 micro-USB 不是 CDC 虚拟串口；调参/看波形时必须外接 USB-TTL（RX 接 PG14，GND 共地）。

11.5 LED 心跳模式约定

为便于无串口线时肉眼判断系统状态，src/main.cpp 采用以下模式：

11.6 遗留待验证

• USB-TTL 线连接后复核 VOFA+ 串口输出。
• 里程碑 2 的串口部分待 USB-TTL 线到位后完成最终验收。

10. AI agent 工作方式约定

• 每个里程碑完成后停下汇报验收结果，不要连续推进多个里程碑
• 引脚定义以 src/board_pins.h 为准；如需改动，必须同时更新 src/hal_init.cpp 和本 AGENTS.md 的 §3.3
• 改控制逻辑前必须先保证遥测输出正常（先看得见，再调得动）
• 不要未经用户确认就放宽 can_motor.cpp 中的 ±3000 电流软限幅
• 保持 HAL 初始化集中、可读；如需新增外设，优先扩展 src/hal_init.cpp，避免拆散到多个文件