# 平衡轮足机器人使用指南

本指南提供详细的操作步骤，帮助您快速上手使用平衡轮足机器人系统。

## 目录

- [环境准备](#环境准备)
- [编译与烧录](#编译与烧录)
- [系统启动](#系统启动)
- [操作说明](#操作说明)
- [调试与监控](#调试与监控)
- [参数调优](#参数调优)
- [故障排查](#故障排查)

---

## 环境准备

### 1. 硬件准备

**必需设备**：
- STM32H723 控制板
- ST-Link V2/V3 或 J-Link 调试器
- 电池（推荐 24V 锂电池）
- 大疆 DR16 遥控器
- USB 转 TTL 串口模块（用于 CLI 调试）
- 各类电机（LZ、LK、DM）

**可选设备**：
- VOFA+ 数据可视化软件
- 示波器
- 万用表

### 2. 软件准备

**开发工具**：
- [Keil MDK-ARM 5.x](https://www.keil.com/download/product/)（推荐）
- 或 [VS Code](https://code.visualstudio.com/) + [Cortex-Debug](https://marketplace.visualstudio.com/items?itemName=marus25.cortex-debug)
- [STM32CubeMX](https://www.st.com/zh/development-tools/stm32cubemx.html)

**调试工具**：
- [VOFA+](https://www.vofa.plus/) - 数据可视化
- 串口调试助手（如 SecureCRT、XShell、Putty）

**编译工具链**（如使用 CMake 编译）：
- ARM GCC Toolchain: [下载地址](https://developer.arm.com/downloads/-/gnu-rm)
- CMake: [下载地址](https://cmake.org/download/)

### 3. 驱动安装

1. **ST-Link 驱动**：
   - 下载 [ST-Link 驱动包](https://www.st.com/zh/development-tools/stsw-link009.html)
   - 安装完成后，设备管理器中应能看到 ST-Link 设备

2. **串口驱动**（如使用 CH340）：
   - 下载并安装对应芯片的 USB 串口驱动

---

## 编译与烧录

### 方法一：使用 Keil MDK

#### 步骤 1：打开项目

1. 启动 Keil MDK
2. 打开项目文件：`MDK-ARM/CtrBoard-H7_ALL.uvprojx`

#### 步骤 2：配置编译选项

1. 点击菜单栏 `Project` → `Options for Target`
2. 在 `Target` 选项卡中确认：
   - MCU: STM32H723XG
   - ARM Compiler: 版本 6.x 或更高
3. 在 `Debug` 选项卡中选择调试器：
   - 使用 `ST-Link Debugger` 或 `J-Link Debugger`
4. 在 `Flash Download` 选项卡中确认：
   - Programming Algorithm: STM32H7x3 1MB Flash
   - 选中 `Reset and Run` 选项

#### 步骤 3：编译项目

1. 点击 `Project` → `Build Target`（或按 F7）
2. 等待编译完成，检查是否有错误
3. 成功后会在 `MDK-ARM/CtrBoard-H7_ALL/` 目录生成 `.hex` 文件

#### 步骤 4：烧录程序

1. 连接 ST-Link 到控制板
2. 点击 `Flash` → `Download`（或按 F8）
3. 等待下载完成，提示 "Programming Done"
4. 控制板会自动复位运行

### 方法二：使用 CMake

#### 步骤 1：配置构建环境

```bash
# 进入项目根目录
cd balance_infantry

# 创建并进入构建目录
mkdir build && cd build

# 配置 CMake
cmake ..
```

#### 步骤 2：编译项目

```bash
# 使用 4 个线程并行编译
make -j4

# 编译完成后，生成的文件位于 build 目录
```

#### 步骤 3：烧录程序

使用 OpenOCD 或 ST-Link 工具烧录：

```bash
# 使用 OpenOCD 烧录（需要先安装 openocd）
openocd -f interface/stlink.cfg -f target/stm32h7x.cfg \
        -c "program CtrBoard-H7_ALL.hex verify reset exit"
```

或使用 ST-LINK Utility 图形界面工具。

---

## 系统启动

### 1. 首次启动检查清单

⚠️ **重要**：首次启动前请务必完成以下检查：

- [ ] 电池电压正常（推荐 22V-28V）
- [ ] 所有电机连接正确，没有短路
- [ ] IMU 安装稳固，方向正确
- [ ] 遥控器已配对，电量充足
- [ ] 电机 ID 配置正确（参考 `User/device/` 目录下电机驱动）
- [ ] 机械结构紧固，无松动
- [ ] 运动空间充足，无障碍物

### 2. 上电启动流程

1. **连接电源**：
   - 将电池连接到电源输入端
   - 确认电压稳定在正常范围

2. **系统初始化**：
   - 系统会自动运行初始化任务（`Task_Init`）
   - LED 指示灯开始闪烁
   - 电机进行零点校准

3. **等待就绪**：
   - 等待约 2-3 秒，系统进入待机状态
   - LED 常亮或按固定频率闪烁（根据代码配置）

4. **连接遥控器**：
   - 打开 DR16 遥控器
   - 确认遥控器与接收器已连接
   - 遥控器 LED 常绿表示连接成功

### 3. 模式选择

系统支持多种控制模式，通过遥控器切换：

| 模式 | 说明 | 切换方式 |
|------|------|---------|
| 放松模式 | 电机不输出，用于调试 | 遥控器左摇杆按下 |
| 复位模式 | 机器人回到初始位置 | 遥控器特定按键 |
| 平衡模式 | 机器人自我平衡 | 遥控器特定按键 |
| 小陀螺模式 | 陀螺仪辅助平衡 | 遥控器特定按键 |

**具体按键配置请参考 `User/task/rc.c` 文件。**

---

## 操作说明

### 1. 基本运动控制

#### 遥控器操作（DR16）

```
左摇杆：
  - 前后推拉：前进/后退
  - 左右推拉：平移
  - 按下：模式切换

右摇杆：
  - 前后推拉：云台 Pitch 轴控制
  - 左右推拉：云台 Yaw 轴控制
  - 按下：发射机构控制

左拨轮：调节目标高度

右拨轮：调节运动速度
```

#### 注意事项

1. **首次操作**：
   - 从低速开始，逐步熟悉控制手感
   - 保持足够的缓冲空间
   - 随时准备切换到放松模式

2. **平衡模式**：
   - 机器人会自动保持平衡
   - 输入较小的运动指令测试响应
   - 观察机器人姿态是否稳定

3. **高度控制**：
   - 使用左拨轮调整目标高度
   - 高度变化应平缓，避免突变

### 2. 云台控制

#### 云台 Yaw 轴控制

- 使用遥控器右摇杆左右推拉
- 控制底盘旋转角度
- 平稳转动，避免快速摆动

#### 云台 Pitch 轴控制

- 使用遥控器右摇杆前后推拉
- 控制发射机构俯仰角
- 注意角度限制，避免碰撞

### 3. 发射机构控制

#### 开启发射机构

1. 切换到发射模式
2. 按下右摇杆（或特定按键）启动摩擦轮
3. 等待摩擦轮达到稳定转速
4. 再次按键发射弹丸

#### 注意事项

- ⚠️ 发射前确保前方安全，无人员
- ⚠️ 不使用时及时关闭发射机构
- 定期检查摩擦轮磨损情况

### 4. 跳跃功能

#### 触发跳跃

跳跃功能仅在 `CHASSIS_MODE_WHELL_LEG_BALANCE` 模式下可用。

**跳跃流程**：

1. **蓄力阶段（JUMP_CROUCH）**：
   - 机器人降低重心，缩短腿长
   - 蓄力时间：`jump_params.crouch_time_ms`（默认配置）

2. **起跳阶段（JUMP_LAUNCH）**：
   - 机器人发力向下，产生向上推力
   - 发力时间：`jump_params.launch_time_ms`

3. **收腿阶段（JUMP_RETRACT）**：
   - 腿部收缩，准备落地
   - 收腿时间：`jump_params.retract_time_ms`

4. **落地阶段（JUMP_LAND）**：
   - 缓冲着陆，恢复平衡
   - 缓冲时间：`jump_params.land_time_ms`

#### 跳跃参数调整

在 `User/module/config.c` 中修改跳跃参数：

```c
.jump_params = {
    .crouch_time_ms = 500,       // 蓄力时间 (ms)
    .launch_time_ms = 100,      // 起跳发力时间 (ms)
    .retract_time_ms = 200,     // 收腿时间 (ms)
    .land_time_ms = 300,        // 落地缓冲时间 (ms)
    .crouch_leg_length = 0.3,   // 蓄力时腿长 (m)
    .launch_force = 500.0,      // 起跳力 (N)
    .retract_leg_length = 0.2,  // 收腿时腿长 (m)
    .retract_force = -100.0,    // 收腿前馈力 (N)
},
```

**调试建议**：
- 先调整蓄力时间和腿长，找到合适的蓄力状态
- 逐步增加起跳力，避免跳得太高
- 根据实际表现调整各阶段时间

---

## 调试与监控

### 1. CLI 命令行调试

#### 连接方式

1. 使用 USB 转 TTL 串口模块连接到开发板的串口
2. 串口参数配置：
   - 波特率：115200
   - 数据位：8
   - 停止位：1
   - 校验位：无
3. 使用串口调试工具打开对应 COM 口

#### 常用命令

```bash
# 显示帮助信息
help

# 查看系统状态
status
# 输出示例：
# Battery: 24.5V (80%)
# CPU Temp: 45°C
# Tasks: Running normally

# 电机控制
motor
# 子命令：
#   list          列出所有电机状态
#   calibrate [id] 校准指定电机
#   enable [id]   启用指定电机
#   disable [id]  禁用指定电机

# 底盘控制
chassis
# 子命令：
#   mode [mode]   设置底盘模式
#   balance       进入平衡模式
#   relax         进入放松模式

# 云台控制
gimbal
# 子命令：
#   reset         复位云台
#   calibrate     校准云台

# 监控任务信息
monitor
# 输出各任务运行频率、栈使用情况
```

### 2. VOFA+ 数据可视化

#### 配置步骤

1. 打开 VOFA+ 软件
2. 新建工程，选择 "JustFloat" 协议
3. 配置串口：
   - 波特率：115200
   - 数据位：8
   - 停止位：1
4. 点击连接按钮

#### 可视化数据

系统会自动发送以下数据：

- IMU 数据：加速度、陀螺仪、欧拉角
- 电机数据：位置、速度、电流
- 控制输出：LQR 输出、VMC 输出
- 系统状态：电池电压、CPU 温度

#### 数据导出

- 点击 "录制" 按钮开始记录数据
- 停止录制后，数据可导出为 CSV 格式
- 使用 Excel、MATLAB 等工具分析数据

### 3. 监控任务

`Task_monitor` 实时监控系统状态，包括：

**监控内容**：
- 电池电压和电量百分比
- CPU 温度
- 各任务运行频率
- 任务栈使用情况
- 设备在线状态

**查看方式**：
- 通过 CLI 命令 `monitor`
- 或通过 VOFA+ 实时查看

**正常指标**：
- 电池电压：22V - 28V
- CPU 温度：< 70°C
- 任务频率：稳定在设定值（如 500Hz）
- 栈使用率：< 80%

---

## 参数调优

### 1. LQR 参数调优

#### LQR 增益矩阵

LQR 控制器的增益矩阵存储在 `User/module/config.c` 中：

```c
LQR_GainMatrix_t lqr_gains = {
    // K 矩阵第一行（轮毂力矩 T）
    .k11_coeff = { ... },  // theta 的增益
    .k12_coeff = { ... },  // d_theta 的增益
    .k13_coeff = { ... },  // x 的增益
    .k14_coeff = { ... },  // d_x 的增益
    .k15_coeff = { ... },  // phi 的增益
    .k16_coeff = { ... },  // d_phi 的增益
    
    // K 矩阵第二行（髋关节力矩 Tp）
    .k21_coeff = { ... },
    .k22_coeff = { ... },
    .k23_coeff = { ... },
    .k24_coeff = { ... },
    .k25_coeff = { ... },
    .k26_coeff = { ... },
};
```

#### 调优方法

**步骤 1：确定目标**
- 平衡性：机器人能稳定平衡
- 响应性：对指令响应迅速
- 稳定性：无明显振荡

**步骤 2：逐步调整**
1. 从小增益开始，逐步增大
2. 先调整 theta 和 phi 的增益（姿态控制）
3. 再调整 x 的增益（位置控制）
4. 最后调整角速度的增益（阻尼）

**步骤 3：测试验证**
- 每次调整后重新编译烧录
- 测试平衡性能
- 使用 VOFA+ 观察响应曲线

**调优技巧**：
- 增益过小：响应慢，容易失衡
- 增益过大：振荡，不稳定
- 角速度增益影响阻尼，防止振荡
- 位置增益影响响应速度

### 2. PID 参数调优

#### PID 控制器位置

系统中有多个 PID 控制器：
- Yaw 轴 PID（`pid.yaw`）
- Roll 轴 PID（`pid.roll`）
- 摆力矩 PID（`pid.tp`）
- 腿长 PID（`pid.leg_length`）
- 摆角 PID（`pid.leg_theta`）

#### 调优步骤（经典方法）

**步骤 1：设置初始值**
```
Kp = 0
Ki = 0
Kd = 0
```

**步骤 2：调整 Kp**
- 逐步增大 Kp
- 直到系统开始振荡
- 将 Kp 减半（约为临界值的 50%-70%）

**步骤 3：调整 Kd**
- 增大 Kd
- 减小振荡
- 直到振荡消失
- 保持适当的阻尼

**步骤 4：调整 Ki**
- 增大 Ki
- 消除稳态误差
- 避免积分饱和

**步骤 5：微调**
- 小幅调整三个参数
- 找到最佳平衡点

#### 参数配置示例

```c
KPID_Params_t yaw = {
    .kp = 5.0,
    .ki = 0.1,
    .kd = 0.5,
    .integral_limit = 10.0,
    .output_limit = 10.0,
};
```

### 3. VMC 参数调优

#### VMC 运动学参数

```c
VMC_Param_t vmc_param = {
    .hip_length = 0.15,      // 髋关节间距 (m)
    .leg_1 = 0.2,            // 大腿前端长度 (m)
    .leg_2 = 0.2,            // 大腿后端长度 (m)
    .leg_3 = 0.25,           // 小腿长度 (m)
    .leg_4 = 0.1,            // 小腿前端长度 (m)
    .wheel_radius = 0.06,    // 轮子半径 (m)
    .wheel_mass = 0.5,       // 轮子质量 (kg)
};
```

#### 调优要点

1. **运动学参数**：
   - 根据实际机械尺寸精确测量
   - 使用 MATLAB 工具（`utils/Simulation-master/`）验证
   - 确保单位一致（全部使用米）

2. **滤波参数**：
   - 调整 `low_pass_cutoff_freq` 优化信号质量
   - 太高频：噪声大
   - 太低频：响应慢

### 4. 使用工具辅助调优

项目提供 MATLAB 仿真工具：

1. **LQR 增益计算**：
   - 运行 `utils/6. 综合运动控制验证/LQR_K.m`
   - 输入系统参数
   - 自动计算最优 LQR 增益

2. **运动学仿真**：
   - 使用 `utils/Simulation-master/` 中的 MATLAB 模型
   - 验证五连杆运动学
   - 测试不同参数组合

3. **参数优化脚本**：
   - 使用 `utils/k_calc/` 中的脚本
   - 批量测试参数
   - 找到最优解

---

## 故障排查

### 1. 系统无法启动

**症状**：上电后无反应，LED 不亮

**可能原因**：
- 电池电压不足或未连接
- 电源线路故障
- MCU 未烧录程序

**解决方法**：
1. 检查电池电压是否正常（22V-28V）
2. 检查电源线路是否接触良好
3. 重新烧录程序

### 2. 电机不转动

**症状**：系统运行正常，但电机不转动

**可能原因**：
- 电机 ID 配置错误
- CAN 通信故障
- 电机未使能
- 电机驱动板故障

**解决方法**：
1. 使用 CLI 命令 `motor list` 检查电机状态
2. 检查 CAN 线连接
3. 确认电机 ID 配置正确
4. 使用 `motor enable [id]` 使能电机
5. 检查电机驱动板供电

### 3. IMU 数据异常

**症状**：姿态角不稳定或数据异常

**可能原因**：
- IMU 供电不稳定
- IMU 安装不牢固
- SPI 通信错误
- IMU 未校准

**解决方法**：
1. 检查 IMU 供电电压（3.3V）
2. 重新安装 IMU，确保稳固
3. 检查 SPI 线连接
4. 使用 CLI 命令校准 IMU
5. 更新 AHRS 算法参数

### 4. 机器人无法平衡

**症状**：机器人启动平衡模式后立即倒下

**可能原因**：
- LQR 参数不合理
- IMU 数据异常
- 电机响应不足
- 机械结构问题

**解决方法**：
1. 检查 IMU 数据是否正常
2. 使用 VOFA+ 观察姿态角变化
3. 重新调整 LQR 参数（参考参数调优章节）
4. 检查电机输出能力
5. 检查机械结构，确保无松动

### 5. 通信故障

**症状**：遥控器失联或 VOFA+ 无法连接

**可能原因**：
- 遥控器未配对
- 串口参数错误
- 通信线路故障
- 任务未正常运行

**解决方法**：
1. 重新配对 DR16 遥控器
2. 检查串口参数（115200, 8N1）
3. 检查通信线路
4. 使用 CLI 命令 `monitor` 查看任务状态

### 6. 编译错误

**症状**：编译时出现错误

**可能原因**：
- 工具链版本不兼容
- CMake 版本过低
- 路径问题
- 代码语法错误

**解决方法**：
1. 更新 ARM GCC 到最新版本
2. 确认 CMake 版本 >= 3.22
3. 检查项目路径是否有中文或特殊字符
4. 查看错误信息，定位代码问题
5. 清理构建目录后重新编译：`make clean && make`

### 7. 跳跃功能异常

**症状**：跳跃后无法落地或姿态失控

**可能原因**：
- 跳跃参数不合理
- 腿长限位设置错误
- 落地检测失效
- 控制器响应不足

**解决方法**：
1. 降低起跳力，从较小值开始测试
2. 调整各阶段时间参数
3. 检查腿长限位是否合理
4. 优化落地缓冲参数
5. 使用 VOFA+ 观察跳跃过程数据

### 获取更多帮助

如果以上方法无法解决问题：

1. **查看日志**：
   - 使用 VOFA+ 记录故障时的数据
   - 使用 CLI 命令查看详细状态

2. **查阅文档**：
   - 检查代码注释
   - 查看 `utils/` 目录中的技术文档

3. **提交 Issue**：
   - 在项目仓库提交详细的问题描述
   - 包含：硬件型号、软件版本、错误日志、重现步骤

---

## 附录

### A. 术语表

| 术语 | 全称 | 说明 |
|------|------|------|
| LQR | Linear Quadratic Regulator | 线性二次调节器 |
| VMC | Virtual Model Control | 虚拟模型控制 |
| IMU | Inertial Measurement Unit | 惯性测量单元 |
| PID | Proportional-Integral-Derivative | 比例-积分-微分控制器 |
| CAN | Controller Area Network | 控制器局域网 |
| FDCAN | Flexible Data-rate CAN | 灵活数据速率 CAN |
| CLI | Command Line Interface | 命令行接口 |
| AHRS | Attitude and Heading Reference System | 姿态和航向参考系统 |

### B. 串口引脚定义

| 串口 | TX | RX | 功能 |
|------|----|----|------|
| USART1 | PA9 | PA10 | CLI 调试 |
| USART2 | PA2 | PA3 | VOFA+ 数据 |
| USART3 | PB10 | PB11 | 视觉通信 |
| UART4 | PA0 | PA1 | 遥控接收 |

### C. 电机 ID 分配

| 电机类型 | ID | 位置 | 用途 |
|---------|----|----|------|
| LZ 电机 | 1-4 | 关节 | 髋关节控制 |
| LK 电机 | 1-2 | 轮子 | 驱动轮控制 |
| DM 电机 | 1 | 云台 | Yaw 轴控制 |
| DM 电机 | 2 | 云台 | Pitch 轴控制 |

### D. 常用按键映射

| 按键 | 功能 | 说明 |
|------|------|------|
| 左摇杆按下 | 模式切换 | 在不同控制模式间切换 |
| 右摇杆按下 | 发射控制 | 控制发射机构开关 |
| 左拨轮 | 高度调节 | 调节机器人目标高度 |
| 右拨轮 | 速度调节 | 调节运动速度上限 |

---

**祝您调试顺利！**

如有问题，请参考 README.md 中的联系方式获取支持。