# 平衡轮足机器人电控系统

基于 STM32H723 的平衡轮腿机器人电控系统，采用 FreeRTOS 实时操作系统，支持 LQR、VMC 等先进控制算法。

## 项目简介

本项目是一个高性能的平衡轮腿机器人电控系统，实现了机器人的平衡控制、运动控制、云台控制、发射机构控制等功能。系统采用模块化设计，具有良好的可扩展性和可维护性。

### 主要特性

- ✅ **高性能硬件平台**：STM32H723 微控制器，480MHz 主频
- ✅ **实时操作系统**：FreeRTOS，多任务并发处理
- ✅ **先进控制算法**：
  - LQR（线性二次调节器）平衡控制
  - VMC（虚拟模型控制）五连杆运动学解算
  - PID 控制器
  - 卡尔曼滤波器
- ✅ **多种功能模块**：
  - 平衡底盘控制（轮腿结构）
  - 云台控制（Yaw/Pitch）
  - 发射机构控制
  - 遥控接收（DR16）
  - AI 视觉系统
- ✅ **丰富的外设接口**：
  - CAN/FDCAN 总线
  - SPI 接口
  - UART 串口
  - PWM 输出
  - ADC 采集
- ✅ **调试工具**：
  - CLI 命令行接口
  - VOFA+ 数据可视化
  - 监控任务

## 硬件架构

### 主控板
- **型号**：STM32H723XG
- **主频**：480MHz
- **Flash**：1MB
- **RAM**：1.4MB

### 传感器
- **IMU**：BMI088（加速度计 + 陀螺仪）
- **遥控器**：大疆 DR16
- **电机**：
  - LZ 直流无刷电机（关节电机）
  - LK 直流无刷电机（轮子电机）
  - DM 直流无刷电机（云台电机）

### 通信接口
- **CAN/FDCAN**：电机通信、视觉通信
- **SPI**：IMU 数据读取
- **UART**：遥控接收、CLI、VOFA+

## 软件架构

### 目录结构

```
├── Core/                 # STM32 HAL 库生成的核心代码
│   ├── Inc/             # 头文件
│   └── Src/             # 源文件
├── Drivers/              # STM32 驱动库
├── User/                # 用户代码
│   ├── bsp/             # 板级支持包
│   │   ├── can.h/.c     # CAN/FDCAN 驱动
│   │   ├── uart.h/.c    # UART 驱动
│   │   ├── spi.h/.c     # SPI 驱动
│   │   ├── gpio.h/.c    # GPIO 驱动
│   │   ├── flash.h/.c   # Flash 操作
│   │   └── ...
│   ├── component/       # 通用组件
│   │   ├── lqr.h/.c     # LQR 控制器
│   │   ├── vmc.h/.c     # VMC 虚拟模型控制
│   │   ├── pid.h/.c     # PID 控制器
│   │   ├── kalman_filter.h/.c  # 卡尔曼滤波
│   │   ├── ahrs.h/.c    # 姿态解算
│   │   ├── filter.h/.c  # 滤波器
│   │   ├── limiter.c    # 限幅器
│   │   └── ...
│   ├── device/          # 设备驱动
│   │   ├── motor_lz.h/.c     # LZ 电机驱动
│   │   ├── motor_lk.h/.c     # LK 电机驱动
│   │   ├── motor_dm.h/.c     # DM 电机驱动
│   │   ├── dr16.h/.c         # 遥控器驱动
│   │   ├── bmi088.h/.c       # IMU 驱动
│   │   ├── vision_bridge.c   # 视觉通信
│   │   ├── vofa.h/.c         # VOFA+ 调试
│   │   └── ...
│   ├── module/          # 功能模块
│   │   ├── balance_chassis.h/.c  # 平衡底盘
│   │   ├── gimbal.h/.c          # 云台
│   │   ├── shoot.h/.c           # 发射机构
│   │   └── config.h/.c          # 配置管理
│   └── task/            # 任务管理
│       ├── user_task.h/.c  # 任务框架
│       ├── rc.c            # 遥控任务
│       ├── ctrl_chassis.c  # 底盘控制任务
│       ├── ctrl_gimbal.c   # 云台控制任务
│       ├── ctrl_shoot.c    # 发射控制任务
│       ├── ai.c            # AI 任务
│       ├── monitor.c       # 监控任务
│       ├── cli.c           # CLI 任务
│       └── ...
├── utils/               # 工具和仿真
├── MDK-ARM/            # Keil MDK 工程文件
├── build/              # CMake 构建目录
└── CMakeLists.txt      # CMake 构建配置
```

### 任务调度

系统采用 FreeRTOS 实时多任务调度，主要任务包括：

| 任务名称 | 频率 | 功能描述 |
|---------|------|---------|
| Task_rc | 500Hz | 遥控器数据处理 |
| Task_atti_esit | - | 姿态估计 |
| Task_ctrl_chassis | 500Hz | 底盘控制 |
| Task_ctrl_gimbal | 500Hz | 云台控制 |
| Task_ctrl_shoot | 500Hz | 发射控制 |
| Task_ai | 500Hz | AI 视觉处理 |
| Task_monitor | 500Hz | 系统监控 |
| Task_vofa | 500Hz | VOFA+ 数据发送 |
| Task_cli | - | CLI 命令行处理 |
| Task_blink | 500Hz | LED 指示灯 |

### 核心算法

#### 1. LQR 控制

线性二次调节器（Linear Quadratic Regulator）用于实现轮腿机器人的平衡控制。

**状态向量**：
- theta：摆杆角度
- d_theta：摆杆角速度
- x：驱动轮位移
- d_x：驱动轮速度
- phi：机体俯仰角
- d_phi：机体俯仰角速度

**控制输出**：
- T：轮毂力矩
- Tp：髋关节力矩

#### 2. VMC 控制

虚拟模型控制（Virtual Model Control）实现五连杆运动学解算和力矩分配。

**功能**：
- 五连杆正运动学解算（关节角度 → 足端位置）
- 五连杆逆运动学解算（虚拟力 → 关节力矩）
- 等效摆动杆参数计算
- 地面接触检测

#### 3. 平衡控制流程

```
遥控输入 → 运动指令 → LQR 控制 → VMC 解算 → 电机输出 → 平衡控制
              ↓
         姿态反馈
```

## 编译环境

### 推荐开发环境

1. **IDE**：
   - Keil MDK-ARM 5.x
   - VS Code + Cortex-Debug 插件

2. **编译工具链**：
   - ARM GCC Toolchain (arm-none-eabi-gcc)
   - CMake 3.22+

3. **调试工具**：
   - ST-Link V2/V3
   - J-Link

### 依赖库

- STM32 HAL 库
- FreeRTOS
- CMSIS DSP 库

## 快速开始

### 1. 克隆项目

```bash
git clone ssh://git@gitea.qutrobot.top:222/robofish/rm_balance.git
cd balance_infantry
```

### 2. 使用 Keil MDK 编译

1. 打开 `MDK-ARM/CtrBoard-H7_ALL.uvprojx`
2. 选择正确的编译配置（Debug/Release）
3. 点击编译（F7）
4. 下载到开发板（F8）

### 3. 使用 CMake 编译

```bash
# 创建构建目录
mkdir -p build && cd build

# 配置 CMake
cmake ..

# 编译
make -j4

# 生成 HEX 文件位于 build 目录
```

### 4. 使用 VS Code 开发

项目已配置 `.clangd`，支持：
- 智能代码补全
- 语法检查
- 跳转到定义
- 重构功能

## 配置说明

### 参数配置

主要参数配置文件位于 `User/module/config.c`，包括：

- **底盘参数**（`Chassis_Params_t`）：
  - 电机参数
  - VMC 参数
  - LQR 增益矩阵
  - PID 参数
  - 跳跃参数

- **云台参数**（`Gimbal_Params_t`）：
  - 电机参数
  - PID 参数

- **发射参数**（`Shoot_Params_t`）：
  - 摩擦轮参数
  - 拨弹轮参数

### 配置文件格式

项目使用 YAML 格式的配置文件（部分模块）：
- `User/task/config.yaml`
- `User/bsp/bsp_config.yaml`
- `User/component/component_config.yaml`
- `User/device/device_config.yaml`

## 调试工具

### 1. CLI 命令行

通过串口连接，可使用以下命令：

```bash
help                    # 显示帮助信息
status                  # 显示系统状态
motor                   # 电机控制
chassis                 # 底盘控制
gimbal                  # 云台控制
```

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

支持实时数据监控和绘图，配置串口参数后即可使用。

### 3. 监控任务

`Task_monitor` 实时监控系统状态，包括：
- 电池电压
- CPU 温度
- 任务运行频率
- 栈使用情况

## 注意事项

### 安全事项

⚠️ **重要**：
1. 首次运行前务必检查电机方向和限位
2. 调试时使用低电压，避免失控
3. 确保 IMU 安装正确，校准零点
4. 测试跳跃功能时做好防护

### 常见问题

1. **编译错误**：
   - 检查 ARM GCC 工具链是否正确安装
   - 确认 CMake 版本 >= 3.22

2. **下载失败**：
   - 检查 ST-Link/J-Link 连接
   - 确认 BOOT 模式设置正确

3. **平衡控制失效**：
   - 检查 IMU 数据是否正常
   - 确认 LQR 参数是否合理
   - 验证电机反馈数据

## 贡献指南

欢迎提交 Issue 和 Pull Request！

## 许可证

本项目采用 MIT 许可证。

## 联系方式

- 项目地址：ssh://git@gitea.qutrobot.top:222/robofish/rm_balance.git

---

**祝您使用愉快！**