AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add DEBUG mode with host-side PID tuning, motor enable/disable protection

Browse Source 
- Add DEBUG mode (parallel to AUTO/MANUAL) for real-time PID parameter tuning
- New 0xA8 debug frame: 12 floats for vision/motor angle PID tuning params
- DMMotorSetMode: send zero-velocity before disable, clear PID to prevent jumps
- motor_enable topic: pub from robot_cmd, sub in gimbal, controls motor on/off
- motor_pid topic: pub from robot_cmd, sub in gimbal, updates motor angle PID
- Rename DM motor enums (DM_CMD_ENABLE_MODE/DISABLE_MODE, DM_Motor_Ctrl_Mode_e)
- SwitchToAuto/Manual: re-enable motors when switching back from DEBUG mode
- Static-allocate enable_msg/pid_tune in GimbalTask to avoid stack churn
- Add docs/host_comm_guide.md with Python Tkinter tuning GUI example
- Remove ARCHITECTURE.md
This commit is contained in:
李傲琦
2026-05-31 03:49:41 +08:00
parent 77a3aeb25c
commit 9b8e204ff7
10 changed files with 747 additions and 643 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ARCHITECTURE.md
-310
View File
@@ -1,310 +0,0 @@
# 项目架构：Basic_Framework_MC02 (DM 云台控制系统)
## 概览
基于 **STM32H723** (Cortex-M7, 480MHz) 的达妙电机云台控制系统。采用 FreeRTOS，通过 IMU 反馈实现角度闭环，通过串口接收上位机指令实现视觉追踪。项目源自 HNU YueLu EC RoboMaster 项目，经重构精简为纯云台控制。
---
## 一、硬件与通信
| 资源 | 用途 |
|------|------|
| **FDCAN1** | 电机控制总线 (DM-G6220 yaw, DM-J4310-2EC pitch) |
| **USART10** | 上位机通信 (master_machine 协议) |
| **USART7** | VoFA 调试输出 |
| **USART1** | 板间通信 (BoardComm) |
| **SPI2** | BMI088 IMU 传感器 |
---
## 二、分层架构
```
┌──────────────────────────────────────────────┐
│ application/ 应用层 │
│ robot.c → robot_cmd → gimbal │
├──────────────────────────────────────────────┤
│ modules/ 模块层 │
│ motor, imu, master_machine, algorithm, ... │
├──────────────────────────────────────────────┤
│ bsp/ 板级支持包 │
│ can, usart, spi, dwt, log, gpio │
├──────────────────────────────────────────────┤
│ Drivers/ HAL 驱动层 (CubeMX 生成) │
│ STM32H7xx_HAL, CMSIS, FreeRTOS │
└──────────────────────────────────────────────┘
```
---
## 三、启动流程
```
main()
├── HAL_Init() → SystemClock_Config() → MX_*_Init() // CubeMX 外设初始化
└── RobotInit()
├── BSPInit() // DWT + Log
├── RobotCMDInit() // pub-sub, 视觉PID, 串口初始化
├── GimbalInit() // IMU + DM 电机初始化
└── OSTaskInit() // 创建 4 个 RTOS 任务
```
### FreeRTOS 任务
| 任务 | 频率 | 优先级 | 功能 |
|------|------|--------|------|
| **INS Task** | 1kHz | AboveNormal | BMI088 读取 → 四元数 EKF 姿态解算 |
| **Motor Task** | 1kHz | Normal | DM 电机角度 PID → CAN 速度指令发送 |
| **Robot Task** | 500Hz | Normal | 上位机命令处理 + 模式切换 + 控制执行 |
| **Daemon Task** | 100Hz | Normal | 模块看门狗 + 蜂鸣器 |
---
## 四、PID 双环架构
```
┌──────────────────────────────────────────────────────┐
│ application/cmd/robot_cmd.c (外环, 500Hz) │
│ │
│ 上位机 0xA5 帧 → error_x/y → KF → vision PID │
│ → 角度增量 → 累计 angle ref │
│ 上位机 0xA6 帧 → yaw_ref/pitch_ref 直接设定 │
│ │
│ ProcessCommand() 处理模式切换 / 激光控制 │
└──────────────────────┬───────────────────────────────┘
│ gimbal_cmd (pub-sub)
▼
┌──────────────────────────────────────────────────────┐
│ application/gimbal/gimbal.c (传递, 500Hz) │
│ │
│ DMMotorSetRef(yaw, cmd.yaw) │
│ DMMotorSetRef(pitch, cmd.pitch) │
│ 反馈: gimbal_feed → gimbal_IMU_data 指针 │
└──────────────────────┬───────────────────────────────┘
│
▼
┌──────────────────────────────────────────────────────┐
│ modules/motor/DMmotor/ (内环, 1kHz) │
│ │
│ IMU 角度反馈 → angle PID → velocity output │
│ → DMMotorSendVelocity() → CAN 速度模式 │
│ output_reverse 控制输出方向 │
└──────────────────────────────────────────────────────┘
```
---
## 五、上位机串口协议
位于 `modules/master_machine/`。三种帧类型，通过帧头区分。
### 自动/视觉追踪数据帧 `0xA5` (10 字节)
| 字节 | 0 | 1-4 | 5-8 | 9 |
|------|---|-----|-----|---|
| 内容 | 0xA5 | error_x (float) | error_y (float) | checksum |
### 手动控制数据帧 `0xA6` (11 字节)
| 字节 | 0 | 1 | 2-5 | 6-9 | 10 |
|------|---|-----|-----|-----|----|
| 内容 | 0xA6 | zero_force | yaw_ref (float) | pitch_ref (float) | checksum |
### 命令帧 `0xA7` (3 字节)
| 字节 | 0 | 1 | 2 |
|------|---|-----|----|
| 内容 | 0xA7 | cmd | checksum |
命令码：`0x01` 切换到自动模式, `0x02` 切换到手动模式, `0x03` 激光开, `0x04` 激光关
### 数据流
```
上位机 USART10 → DecodeVision() → vision_data (全局结构体)
├── 0xA5 → auto_updated=1, error_x/y
├── 0xA6 → manual_updated=1, zero_force, yaw_ref/pitch_ref
└── 0xA7 → cmd_pending=1, cmd
robot_cmd 每轮:
ProcessCommand() → 模式切换 / 激光
AUTO: vision_data.error_x/y → KF → vision PID → 角度增量
MANUAL: vision_data.yaw_ref/pitch_ref → 直接设定
PubPushMessage(gimbal_cmd) → gimbal 执行
```
---
## 六、DM 电机驱动
位于 `modules/motor/DMmotor/`。基于 DM_6220 参考协议。
### 速度模式 (当前默认)
CAN ID = motor_id + 0x200，发送 float velocity (4 bytes)。
```
DMMotorControl() @ 1kHz:
PIDCalculate(angle_PID, IMU_angle, pid_ref) → velocity
output_reverse? → DMMotorSendVelocity() → CAN
```
### 电机配置
| 电机 | CAN ID | 反馈 ID | 模式 | MIT KP/KD |
|------|--------|---------|------|-----------|
| DM-G6220 (Yaw) | 0x01 | 0xF1 | Velocity | — |
| DM-J4310-2EC (Pitch) | 0x02 | 0xF2 | Velocity | — |
### API
| 函数 | 说明 |
|------|------|
| `DMMotorInit()` | 初始化电机实例 (CAN 注册, PID 初始化, daemon) |
| `DMMotorSetRef()` | 设定角度参考值 (degrees) |
| `DMMotorControl()` | 1kHz 控制循环 (所有电机) |
| `DMMotorSetMode()` | 发送命令 (使能/停止/清零/清除错误) |
| `DMMotorSendVelocity()` | 速度模式 CAN 发送 |
| `DMMotorSendMIT()` | MIT 模式 CAN 发送 |
| `DMMotorSendPosVel()` | 位置-速度模式 CAN 发送 |
---
## 七、模块层
| 分类 | 模块 | 功能 |
|------|------|------|
| **电机** | `motor/DMmotor` | 达妙电机驱动 (MIT/PosVel/Velocity 三模式) |
| | `motor/motor_task` | 电机控制统一调度 (`DMMotorControl`) |
| **姿态** | `imu/ins_task` | 惯导解算 (四元数 EKF, 返回 INS_t 指针) |
| | `imu/BMI088driver` | BMI088 SPI 读取 |
| | `imu/BMI088Middleware` | BMI088 数据换算 |
| **算法** | `algorithm/controller` | PID 控制器 (梯形积分/变速积分/微分先行/输出滤波) |
| | `algorithm/kalman_filter` | 卡尔曼滤波器 (自适应) |
| | `algorithm/QuaternionEKF` | 四元数 EKF (陀螺零偏估计/卡方检验) |
| | `algorithm/crc8/crc16` | CRC 校验 |
| | `algorithm/user_lib` | 数学/工具函数 |
| **通信** | `master_machine` | 上位机通信 (0xA5/0xA6/0xA7 协议, 接收+解析) |
| | `timachine/board_comm` | 板间通信 |
| | `message_center` | Pub-Sub 消息中间件 (链表, FIFO) |
| **系统** | `daemon` | 看门狗 (重载计数+离线回调) |
| | `alarm/buzzer` | 蜂鸣器 |
| | `vofa` | VoFA+ 调试输出 |
---
## 八、BSP 驱动层
实例化注册模式 (不注册不初始化)：
| 模块 | 功能 |
|------|------|
| `bsp_dwt` | DWT 周期计数器 (μs/ms 延时, 时间戳) |
| `bsp_can` | FDCAN 收发 (FIFO+中断) |
| `bsp_usart` | 串口收发 (DMA+环形缓冲区) |
| `bsp_spi` | SPI 通信 |
| `bsp_log` | SEGGER RTT + 串口日志 |
| `bsp_pwm` | PWM 输出 |
---
## 九、数据流
```
┌──────────────┐
│ 上位机 │
│ (USART10) │
└──────┬───────┘
│ 0xA5 / 0xA6 / 0xA7
▼
┌────────────────────────┐
│ RobotCMD │
│ ProcessCommand() │ ← 模式切换/激光
│ VisionTrackingSet() │ ← KF → vision PID → 角度增量
│ ManualModeSet() │ ← 直接角度参考
│ PubPushMessage() │
└───────────┬────────────┘
│ gimbal_cmd
▼
┌────────────────────────┐
│ Gimbal │
│ DMMotorSetRef() │
│ 反馈: gimbal_feed │ ← INS_t* 指针
└───────────┬────────────┘
│ angle ref
▼
┌────────────────────────┐
│ DMMotor │ (1kHz Motor Task)
│ IMU angle → PID │
│ → Velocity mode CAN │
└────────────────────────┘
│
┌───────────┴────────────┐
│ DM-G6220 DM-J4310 │
│ (Yaw) (Pitch) │
└────────────────────────┘
```
---
## 十、关键设计
1. **IMU 反馈替代编码器** — 云台 Yaw/Pitch 使用 INS_t 中的 YawTotalAngle/Pitch 和 Gyro[]
2. **速度模式控制** — MCU 跑角度 PID，电机内部处理速度闭环
3. **伪 Pub-Sub** — 应用间通过 message_center 解耦
4. **Daemon 看门狗** — 每个外设模块独立 DaemonInstance，离线自动回调
5. **BSP 实例化注册** — CAN/串口不注册不初始化
6. **KF + PID 级联** — 视觉误差经 KF 平滑 → vision PID → 角度增量 → 内环角度 PID
7. **模式切换重置** — 切换到自动模式时重置 KF 状态 (x=0, P=diag(5,5)) 和 PID 积分项
8. **串口命令控制** — 模式切换、激光控制均通过 0xA7 命令帧触发
9. **output_reverse** — PID 输出方向统一通过单标志位控制
---
## 十一、项目规模
| 类别 | 数量 |
|------|------|
| C 源文件 | 96 |
| ASM 源文件 | 2 |
| 应用层模块 | 3 (robot, robot_cmd, gimbal) |
| 模块层子模块 | 10 (motor, imu, algorithm, master_machine, message_center, daemon, buzzer, vofa, board_comm) |
| BSP 驱动模块 | 8 |
| FreeRTOS 任务 | 4 |
| 最高控制频率 | 1kHz |
| CPU 频率 | 480MHz |
| FLASH 使用 | ~133 KB / 1 MB (13%) |
| DTCMRAM 使用 | ~59 KB / 128 KB (46%) |
---
## 十二、核心数据类型
```c
// INS 惯导实例 (ins_task.h)
typedef struct {
float q[4]; // 四元数
float Gyro[3]; // 角速度 (rad/s)
float Accel[3]; // 加速度 (m/s²)
float Roll, Pitch, Yaw;
float YawTotalAngle; // 多圈 yaw (degrees)
} INS_t;
// 云台控制指令 (robot_def.h)
typedef struct {
float yaw; // yaw 目标角度 (degrees)
float pitch; // pitch 目标角度 (degrees)
} Gimbal_Ctrl_Cmd_s;
// 上位机数据 (master_machine.h)
typedef struct {
float error_x, error_y; // 自动模式: 归一化误差
uint8_t auto_updated;
uint8_t zero_force; // 手动模式: 零力标志
float yaw_ref, pitch_ref; // 手动模式: 角度参考
uint8_t manual_updated;
uint8_t cmd, cmd_pending; // 命令
float frequency;
} VisionData_t;
```
README.md
+302 -303
View File
@@ -1,356 +1,355 @@
# 电赛-云台-dmmc02
基于达妙MC02的电赛云台开发板代码
带卡尔曼滤波版本
powered by ggllxx
可用于江南大学2025电赛E题作品开源
以下是原框架描述：
------
# YueLu2022/2023 EC basic_framework-dev
湖南大学RoboMaster跃鹿战队2022-2023电控通用嵌入式框架。
<div align=center>
<img src=".assets/yuelu_badge.png"/>
<p>
<img src="https://img.shields.io/badge/version-beta-blue"/>
<img src="https://img.shields.io/badge/license-MIT-green"/>
<img src="https://github.com/HNUYueLuRM/basic_framework/actions/workflows/c-cpp.yml/badge.svg"/>
</p>
<p>
<img src="https://gitee.com/HNUYueLuRM/basic_framework/badge/star.svg"/>
<img src="https://gitee.com/HNUYueLuRM/basic_framework/badge/fork.svg"/>
</p>
<h5><p><font face="consolas">Best RoboMaster embedded EC open-source code ever?</p></h>
</div>
> ***也许不是最好的？但一定是最完整最详细最适合上手的电控开源！***
***==别忘了仓库根目录下的`.Doc`文件夹中有关于配置环境和开发更详尽的说明！==***
- 若无法访问github，戳[gitee仓库](https://gitee.com/hnuyuelurm/basic_framework)
- 若gitee内容被屏蔽，戳[github仓库](https://github.com/HNUYueLuRM/basic_framework)
> 基于basic_framework打造的C++进阶重构版本[***powerful_framework***](https://gitee.com/hnuyuelurm/powerful_framework)现已发布！增加全新的消息交互机制和严格的跨任务数据读写保护，采用了现代构建系统CMake+Ninja以追求极致的编译速度，各种针对嵌入式的编译优化全开，DIY程度进一步提升，更有自定义CMSIS-DSP和Eigen等扩展库支持！快来加入试用/和我们一起开发吧😋
# 项目架构：Basic_Framework_MC02 (DM 云台控制系统)
## 概览
基于 **STM32H723** (Cortex-M7, 480MHz) 的达妙电机云台控制系统。采用 FreeRTOS，通过 IMU 反馈实现角度闭环，通过串口接收上位机指令实现视觉追踪和调参。项目源自 HNU YueLu EC RoboMaster 项目，经重构精简为纯云台控制。
---
## 一、硬件与通信
## 功能介绍和展示
### 起源
这是湖南大学RoboMaster跃鹿战队电控组2022-2023赛季的通用嵌入式控制框架，可用于**机器人主控**、自研模组（imu/测距等各种传感器）、超级电容控制器等。
从目前的RoboMaster开源社区来看，大部分队伍都没有一套规则统一，符合较大规模软件开发原则的框架，有些学校连不同兵种代码都相去甚远，甚至连队伍用于传承的代码注释都寥寥无几，全靠师傅带徒弟言传身教。当然，不乏有广东工业大学DynamicX开源的rm_control这样规范且先进的系统，但基于Linux、ROS、C++的这套软件栈对于新人来说还是过于复杂（但我们很推荐熟悉基本工具的同学使用！）。
### 优势
为此，basic_framework应运而生。该框架旨在打造上手简单、易于移植、高代码复用、层级分明、结构清晰的一套SDK，供队内使用&RM参赛队伍&嵌入式开发者交流学习。通过精心设计的bsp和module支持以及成熟的app示例，该框架可以在短时间内为不同类型和结构的机器人**轻松构建**电控程序，**可扩展性**和**可维护性**相较目前的大部分开源代码和本队的老代码提升巨大。
同时，相较于传统的基于KEIL MDK开发的工作流，我们推出了基于arm-gnu工具链+VSCode/Clion+Ozone&Systemviewer/FreeMaster的**现代化开发工作流**和非**常完善配套教程**，你再也不用面对上个世纪的UI，忍受没有补全和高亮的代码了！在现代化开发工具支持下，将**极大提高电控开发效率和调试效率**。硬件模块测试和整车联调从未**如此简单方便**！
> 用软件开发的思想设计嵌入式系统是一种降维打击
>
> <p align=right>—— 沃兹基·烁德</p>
### 效果展示
![](.assets/allrobot.jpg)
<center>搭载basic_framework的机器人阵容</center>
实战展示：
1. 400HP双云台哨兵30m/s弹速**对抗**2台200HP 15m/s弹速步兵，120w底盘功率
2. 100HP步兵，120w功率，15m/s弹速**对射**，限制射频0.5发/s
![sentry_infantry12](.assets/sentry_infantry12.gif)
<center>展示中的视觉识别与预测算法是基于rm_vision打造的</center>
3. 工程机器人流程化抓取矿石/兑换矿石/救援’
![engineering](.assets/engineering.gif)
4. 平衡步兵机器人
![balance](.assets/balance.gif)
这些机器人的程序均基于basic_framework打造，已在我们的仓库中提供：[HNUYueLuRM](https://gitee.com/hnuyuelurm)
> 更多测试视频可以关注我们的bilibili账号：[湖南大学跃鹿战队](https://space.bilibili.com/522795884)，或在bilibili搜索跃鹿战队，观看我们的比赛视频。
### 可用功能
你可以基于这些良好抽象的功能打造自己的模块或应用。
#### bsp封装
| 功能类别 | 模块 |
| ---------- | ----------------------- |
| 通信类外设 | usart spi i2c can usb |
| 日志 | log flash |
| 功能型 | gpio(exti) pwm adc |
| 辅助 | dwt |
#### 模块封装
| 功能类别 | 模块 |
| -------- | ------------------------------------------------------------ |
| 电机 | DJI、HT海泰04、瓴控LK、步进电机、舵机 |
| 通信 | 多板通信(基于CAN)、seasky协议上位机通信、裁判系统数据/UI/多机、vofa协议、DT7-DR16遥控器 |
| 功能模块 | 蜂鸣器、oled、bmi088、ist8310、超级电容、TFminiPlus、 |
| 应用支持 | 常用算法库、守护线程、消息中心 |
#### 应用封装
作为命令发布主体的robot_cmd
用于步兵、英雄、哨兵、无人机的gimbal
麦克纳姆轮/全向轮底盘的chassis
平衡步兵的底盘balance_chassis
装配了发射机构的机器人的shoot
| 资源 | 用途 |
|------|------|
| **FDCAN1** | 电机控制总线 (DM-G6220 yaw, DM-J4310-2EC pitch) |
| **USART10** | 上位机通信 (master_machine 协议) |
| **USART7** | VoFA 调试输出 |
| **USART1** | 板间通信 (BoardComm) |
| **SPI2** | BMI088 IMU 传感器 |
---
## 二、分层架构
## 架构
总览。
### 软件栈
<img src=".assets/image-20230725153133419.png" alt="image-20230725153133419" style="zoom: 67%;" />
在CubeMX初始化生成的依赖文件基础之上新增了可选的CMSIS-DSP和Segger RTT。
### 设计思想
1. ***首先，总览框架的设计模式。***
框架在结构上分为三层：bsp/module/app。整体使用的设计模式是**结构层级模式**，即每个“类”包含需要使用的底层“类”，通过组装不同的基础模实现更强大的功能。而最顶层的app之间则通过**pub-sub消息机制**进行解耦，使得在编写代码时不会出现相互包含的情况。
我们希望通过bsp对硬件的抽象使得module的编写更为轻松，不需要考虑底层的硬件具体是如何运作的；再通过module的外接模块的抽象，使得app的编写可以通过完全硬件无关的方式考虑，达到*”只阅读module的说明文档就能迅速开发应用 ”*的程度。bsp和module的设计愿景，就是成为人们常说的***中间件**。*
**pub-sub机制的体现**：以本仓库的app层为例，包含了chassis，gimbal，shoot，cmd四个应用，每个应用都对应了机器人上的不同模组。cmd应用负责从机器人控制信号来源（遥控器/上位机/环境传感器）处获取信息并解析成各个**执行单元的实际动作**（电机/舵机/气缸/阀门等的扭矩/速度/位置/角度/开度等），并将此信息**发布**出去。chassis、gimbal、shoot等包含了执行单元的应用则**订阅**这些消息，并通过自己包含的子模块，调用它们的接口实现动作。
**结构层级模式的体现**：以chassis应用为例，chassis中包含了4个底盘电机模块。当chassis收到cmd应用的信息，希望让底盘以1m/s的速度前进。chassis首先根据底盘的类型（舵轮/麦克纳姆轮/全向轮/平衡底盘）以及对应的动力学/运动学解算函数，计算得到每个电机的输目标输入值，此时chassis将输入通过电机模块（motor module）的接口将设定值告知电机。而每个电机模块又有各自的PID计算模块和自身电流&速度&角度传感器的信息，可以计算出最终需要的电流设定值。假设该电机使用CAN协议与电调通信，则电机通过自身包含的CANInstance（bsp_can提供）用于和实际硬件交互，电机模块将设定值电流值或其他指令按照通信协议组织在CAN报文中，通过CANInstance提供的接口，把最终控制数据发送给电调，实现控制闭环。从调用来看，三个层级的**包含关系为chassis∈motor∈bspcan**。
2. ***有了上面的大体认知，我们分别介绍框架的三层结构。***
- **bsp**即板级支持包，提供对开发板外设的软件抽象，让module层能使用和硬件无关的接口（由bsp提供）进行数据处理与交互。
bsp层和ST的HAL为强耦合，与硬件直接绑定。若要向其他的ST芯片移植，基本不需要修改bsp层；若是其他单片机则建议保留**接口设计**，对接口调用进行重现实现。每一种外设的头文件中都定义了一个**XXXInstance**（xxx为外设名），其中包含了使用该外设所需要的所有数据，如发送/接收的数据，长度，id（如果有），父指针（指向module实例的指针，用于回调）等。由于C没有`class`，因此所有bsp的接口都需要传入一个额外的参数：XXXInstance*，用于实现c++的`this`指针以区分具体是哪一个实例调用了接口。
- **module**即模块层，包括了需要开发板硬件外设支持的（一般用于通信）真实**硬件模组**如电机、舵机、imu、测距传感器，和通过软件实现的**算法**如PID、滤波器、状态观测器；还有用于兼容不同控制信息模块（遥控器/ps手柄/图传链路/上位机）的统一接口模块，以及为app层提供数据交互的message center。
module层仍然是基于实例的，一个app会包含多个module的instance。当app便可以用硬件无关的接口使用module，如要求电机以一定速度运动、关闭气阀、给超级电容或上位机发送一些反馈数据等。在有了方便的bsp之后，只需要在你构建的module中包含必须的bsp，然后为app提供合理易用的接口即可。
- **app**是框架层级中最高的部分。目前的框架设计里，会有多个app任务运行在freertos中，当然你也可以根据需要启动一些事件驱动的任务，所有的任务安排都放在`app/robot_task`中。当前的app层仅是一个机器人开发的示例，有了封装程度极高的module，你可以在app完成任何事情。
目前的app设计里，可以兼容多块开发板的情况，通过**条件编译**切换开发板的位置。如步兵机器人可以将主控MCU放在云台上，而超级电容控制板同时作为底盘板。使用CAN/SPI/UART将两者连接，便可以通过**`app/robot_def.h`**中的宏完成设置。可以根据需要，设置更多的开发板（双云台哨兵、工程机器人）。
这套框架可以轻松扩展到所有机器人上，在我们的仓库中，有步兵机器人、平衡步兵机器人、哨兵机器人、英雄机器人、工程机器人以及空中机器人的代码实例，皆按照本框架中的三层结构开发。若设计了新的机器人，只需要在robot_def.h中修改传感器的位置、底盘轮距轴距、拨弹盘容量、弹舱载弹量等参数便可以**立刻实现部署**。
3. ***知道了每个层级的结构之后，我们再谈谈如何进行每层的开发。***
对于bsp和module中每个instance的设计，我们采用了**面向对象**的C风格代码，整个框架也统一了变量和函数命名方式，调用层级和数据流十分清晰（下一个章节也有插图阐述）。
为了避免出现”底层代码包含上层头文件“的情况，我们让bsp层instance在注册时要求module提供数据发送/接收的回调函数指针，从而在发生对应中断或事件时完成对module函数的”反向调用“。事实上，你也可以进一步将这套思想放入app的设计中，当某个事件发生时触发app的任务，而不是将app的任务定时运行（这可以提高运行效率，降低cpu占用）。
bsp和module的instance在初始化时接口皆为**`XXXInstance* XXXRegister(XXX_Init_Config_s* conf)`**，传入该实例所需的config参数，返回一个实例指针（看作this指针），之后要调用模块的功能，传入该指针即可。我们还提供了守护线程，以供module选用，当异常情况发生时在LOG中发送warning、触发蜂鸣器或LED进行声光报警以及错误/离线回调函数，保证系统的鲁棒性和安全性。
而对于app的开发，由于底层接口已经设计的较为完善，不同的机器人可以直接**`fork`** basic_framework的代码，开发app层。当bsp和module有功能更新时，只需要通过git的cherry-pick-commit功能将更新拉取到自己的仓库，**获得动态的热更新而无需手动合并分支！**
```
┌──────────────────────────────────────────────┐
│ application/ 应用层 │
│ robot.c → robot_cmd → gimbal │
├──────────────────────────────────────────────┤
│ modules/ 模块层 │
│ motor, imu, master_machine, algorithm, ... │
├──────────────────────────────────────────────┤
│ bsp/ 板级支持包 │
│ can, usart, spi, dwt, log, gpio │
├──────────────────────────────────────────────┤
│ Drivers/ HAL 驱动层 (CubeMX 生成) │
│ STM32H7xx_HAL, CMSIS, FreeRTOS │
└──────────────────────────────────────────────┘
```
---
## 三、启动流程
```
main()
├── HAL_Init() → SystemClock_Config() → MX_*_Init() // CubeMX 外设初始化
└── RobotInit()
├── BSPInit() // DWT + Log
├── RobotCMDInit() // pub-sub, 视觉PID, 串口初始化
├── GimbalInit() // IMU + DM 电机初始化
└── OSTaskInit() // 创建 4 个 RTOS 任务
```
## 执行顺序与数据流
### FreeRTOS 任务
### 初始化
| 任务 | 频率 | 优先级 | 功能 |
|------|------|--------|------|
| **INS Task** | 1kHz | AboveNormal | BMI088 读取 → 四元数 EKF 姿态解算 |
| **Motor Task** | 1kHz | Normal | DM 电机角度 PID → CAN 速度指令发送 |
| **Robot Task** | 500Hz | Normal | 上位机命令处理 + 模式切换 + 控制执行 |
| **Daemon Task** | 100Hz | Normal | 模块看门狗 + 蜂鸣器 |
![image-20230725153635454](.assets/image-20230725153635454.png)
---
### 任务结构
## 四、PID 双环架构
app、module和bsp都有相应的rtos任务。其中bsp为创建任务提供了封装工具bsp_tools，旨在将复杂的回调函数转移到任务中而不是在中断内执行，以保证系统响应的实时性和数据完整性。有一些module和app根据功能需要会创建定时任务或事件驱动的任务，这些任务都在初始化时注册，并在特定的时刻被唤醒或周期执行。
```
┌──────────────────────────────────────────────────────┐
│ application/cmd/robot_cmd.c (外环, 500Hz) │
│ │
│ AUTO: 上位机 0xA5 帧 → error_x/y → KF → vision PID│
│ → 角度增量 → 累计 angle ref │
│ MANUAL: 上位机 0xA6 帧 → yaw_ref/pitch_ref 直接设定 │
│ DEBUG: 上位机 0xA8 帧 → 实时更新 PID 参数 │
│ │
│ ProcessCommand() 处理模式切换 / 激光控制 │
└──────────────────────┬───────────────────────────────┘
│ gimbal_cmd (pub-sub)
▼
┌──────────────────────────────────────────────────────┐
│ application/gimbal/gimbal.c (传递, 500Hz) │
│ │
│ DMMotorSetRef(yaw, cmd.yaw) │
│ DMMotorSetRef(pitch, cmd.pitch) │
│ 电机使能/失能订阅: motor_enable │
│ 电机角 PID 调参订阅: motor_pid │
│ 反馈: gimbal_feed → gimbal_IMU_data 指针 │
└──────────────────────┬───────────────────────────────┘
│
▼
┌──────────────────────────────────────────────────────┐
│ modules/motor/DMmotor/ (内环, 1kHz) │
│ │
│ IMU 角度反馈 → angle PID → velocity output │
│ → DMMotorSendVelocity() → CAN 速度模式 │
│ DISABLE 时自动清零 PID + 发送零速指令 │
│ output_reverse 控制输出方向 │
└──────────────────────────────────────────────────────┘
```
<img src=".assets/image-20230725152433502.png" alt="image-20230725152433502" style="zoom:50%;" />
---
## 五、上位机串口协议
位于 `modules/master_machine/`。四种帧类型，通过帧头区分。详细说明见 `docs/host_comm_guide.md`。
### 帧类型总览
| 帧头 | 长度 | 用途 |
|------|------|------|
| 0xA5 | 10 | 自动/视觉追踪数据帧 (error_x, error_y) |
| 0xA6 | 11 | 手动控制数据帧 (zero_force, yaw_ref, pitch_ref) |
| 0xA7 | 3 | 命令帧 (模式切换, 激光) |
| 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) |
### 命令码
| cmd | 含义 |
|-----|------|
| 0x01 | 切换到自动/视觉追踪模式 |
| 0x02 | 切换到手动模式 |
| 0x03 | 激光开 |
| 0x04 | 激光关 |
| 0x05 | 切换到调试模式 |
### 工作模式
| 模式 | 值 | 说明 |
|------|-----|------|
| AUTO | 0 | 视觉误差 → KF → vision PID → 角度增量 |
| MANUAL | 1 | 上位机直接下发角度参考值 |
| DEBUG | 2 | 失能电机，接收 PID 参数实时调参 |
### 数据流
![](.assets/dataflow.svg)
```
上位机 USART10 → DecodeVision() → vision_data (全局结构体)
├── 0xA5 → auto_updated=1, error_x/y
├── 0xA6 → manual_updated=1, zero_force, yaw_ref/pitch_ref
├── 0xA7 → cmd_pending=1, cmd
└── 0xA8 → debug_updated=1, 12 个 PID 参数
<center>建议浏览器打开SVG查看<center>
​
robot_cmd 每轮:
ProcessCommand() → 模式切换 / 激光
AUTO: vision_data.error_x/y → KF → vision PID → 角度增量
MANUAL: vision_data.yaw_ref/pitch_ref → 直接设定
DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题
PubPushMessage(gimbal_cmd) → gimbal 执行
```
---
## 六、Pub-Sub 话题
## 开发工具
介绍完整的工作流。
### 工具链
强烈推荐使用arm-gnu工具链进行编译（arm-none-eabi-xxx）。
官方下载地址：[Arm GNU Toolchain Downloads – Arm Developer](https://developer.arm.com/downloads/-/arm-gnu-toolchain-downloads)
我们更推荐使用Msys2进行库和开发工具管理，关于如何使用Msys2请参考：[如何使用本框架](##如何使用本框架)
> 仍然支持使用arm-cc工具链（即keil默认的工具链）进行开发，在cubemx初始化时生成MDK项目即可，然后再手动添加basic_framework的所有头文件和源文件。但非常不建议这样做，arm-cc仅支持单线程编译且编译优化选项远不如arm-gnu多，自定义程度同样不比。~~若你一定要这样做，则可以在VSCode中安装keil assistant插件。~~
### IDE?
使用**VSCode**作为“IDE”，也支持Clion/Visual Studio等现代化IDE（需要自行配置）。需要的插件支持均已经在[VSCode+Ozone使用方法.md](.Doc/VSCode+Ozone使用方法.md)中给出。通过VSCode强大的插件系统、language server以及代码补全高亮助力效率倍增。编译则使用集成的task进行，还可以将开发环境终端加入VSCode进一步提升体验。基本的调试如变量&寄存器查看均已在插件中提供支持，`launch.json`可以进行高自由度的自定义。
`Git`集成与gitlens/gitgraph/githistory额外插件补充让版本管理和协作从未如此简单，`live share`把你的伙伴们聚在一起集思广益，一同对抗困难的bug。更多好用的插件、特性和开发技巧请参考"**如何使用本框架**"章节。
> **不论如何，请不要使用KEIL作为你的代码编辑器。**
### 调试和性能分析
- 基础的调试可以在VSCode中完成。cortex-debug插件的最新版本已经支持多个gdb-server（jlink/stlink/openocd/pyocd）的live watch（动态变量监视）和变量示波器（可视化）。若不是有特别的需求，*请勿使用串口调试器*。
- 有高速变量查看和数据记录、多路数据可视化的需求（如进行pid参数整定、查找难以定位的bug）时，使用**Segger Ozone**。
- FreeMaster也可以作为调试的备选项。
- 基本的、日常性能分析可以通过`bsp_dwt`完成。若要分析关于任务运行和每个函数执行的详细信息和时间，推荐使用**Segger Systemviewer**。
| 话题名 | 发布者 | 订阅者 | 数据类型 |
|--------|--------|--------|----------|
| `gimbal_cmd` | robot_cmd | gimbal | `Gimbal_Ctrl_Cmd_s` (yaw, pitch) |
| `gimbal_feed` | gimbal | robot_cmd | `Gimbal_Upload_Data_s` (INS 指针) |
| `motor_enable` | robot_cmd | gimbal | `Motor_Enable_s` (enable: 0/1) |
| `motor_pid` | robot_cmd | gimbal | `Motor_PID_Tune_s` (yaw/pitch Kp,Ki,Kd) |
---
## 七、DM 电机驱动
位于 `modules/motor/DMmotor/`。基于 DM_6220 参考协议。
## 如何使用本框架
### 速度模式 (当前默认)
仓库中有各种各样的说明文档和使用帮助。
CAN ID = motor_id + 0x200，发送 float velocity (4 bytes)。
### 编译烧录
```
DMMotorControl() @ 1kHz:
PIDCalculate(angle_PID, IMU_angle, pid_ref) → velocity
output_reverse? → DMMotorSendVelocity() → CAN
```
本项目是基于RoboMaster开发板C型的示例，MCU为STM32F407IG，使用了板载的imu bmi-088，驱动标准的步兵机器人：2自由度GM6020云台、m2006电机拨盘+2*m3508电机摩擦轮的发射机构和MG90舵机弹舱盖，以及带有超级电容控制器的4轮麦克纳姆底盘。
### 电机配置
首先在`app/robot_def.h`中根据注释修改开发板和机器人配置，再在各个app中修改初始化配置（如电机id，上位机通信波特率/使用串口或VCP，imu速率，超级电容id等）。
| 电机 | CAN ID | 反馈 ID | 模式 | MIT KP/KD |
|------|--------|---------|------|-----------|
| DM-G6220 (Yaw) | 0x01 | 0xF1 | Velocity | — |
| DM-J4310-2EC (Pitch) | 0x02 | 0xF2 | Velocity | — |
接着根据[VSCode+Ozone使用方法.md](.Doc/VSCode+Ozone使用方法.md)配置好编译下载环境之后（***再次建议使用Msys2+mingw64/ucrt64/clang64的方式配置环境！***），在VSCode中打开项目，点击上方tab页的终端（terminal）->运行构建任务（run build task)，便启动编译，若没有问题，最终会在终端中输出如下信息：
### API
![image-20230725154910307](.assets/image-20230725154910307.png)
| 函数 | 说明 |
|------|------|
| `DMMotorInit()` | 初始化电机实例 (CAN 注册, PID 初始化, daemon) |
| `DMMotorSetRef()` | 设定角度参考值 (degrees) |
| `DMMotorControl()` | 1kHz 控制循环 (所有电机) |
| `DMMotorSetMode()` | 发送命令 (使能/停止/清零/清除错误), DISABLE 时自动清零 PID+零速 |
| `DMMotorSendVelocity()` | 速度模式 CAN 发送 |
| `DMMotorSendMIT()` | MIT 模式 CAN 发送 |
| `DMMotorSendPosVel()` | 位置-速度模式 CAN 发送 |
工具链会预测ram，ccram以及flash的使用情况，并报告最终二进制文件的大小和存放位置。
随后，通过调试器将开发板连接至你的电脑，点击上方tab页的终端（terminal）->运行任务（run task)，选择download_dap or download_jlink（或你自己编写的stlink/ulink/...），便会开始下载，终端或jFlash中会提示擦除、下载、验证的进度。
想要调试，在左侧tab页选择合适的调试选项，按F5或图形界面的绿色小三角形按钮，开始调试。当然，调试器的设置也请参考配置文档，主要是将可执行文件路劲加入环境变量的PATH。
**更详细的开发流程和本仓库工作流的最佳实践，请参照`.Doc/VSCode+Ozone使用方法.md`**，里面介绍了开发所需的前置知识、环境配置，以及工具链原理、使用方法等。
要对本仓库进行开发，务必先阅读`.Doc/架构介绍与开发指南.md`，内含本仓库组织结构的**文件树**。若你希望使用其他工具链或IDE，里面也有相关说明。
### 基本文档
根目录下的README.md即本说明文档，帮助开发者速览本项目。
`.Doc`目录下有**8**个markdown文档，分别为：
- [Bug_Report](.Doc/Bug_Report.md) ：提供了一些提交issues的模板范例，若在使用中出现问题请按照模板提供信息。
- [TODO](.Doc/TODO.md) ：框架后续开发计划和维护说明
- **[VSCode+Ozone使用方法](.Doc/VSCode+Ozone使用方法.md)** ：**重要**，上手必看。介绍了当前开发工作流和传统KEIL开发的不同，先讲解一些与工具链有关的基础知识，然后说明了如何配置开发环境，安装必要的软件和一些”操作“。还涉及了VSCode编辑调试和Ozone示波器&trace功能的使用指南。
- [合理地进行PID参数整定](.Doc/合理地进行PID参数整定.md) ：介绍了如何为PID控制器进行参数整定，包括简单的经验准则和基于模型的前馈控制、扰动消除等方法。
- [如何定位bug](.Doc/如何定位bug.md) ：当嵌入式开发出现bug时，以更高效地方法进行错误定位和复现。简单的调试器使用技巧。
- [必须做&禁止做](.Doc/必须做&禁止做.md) ：字如其名
- **[架构介绍与开发指南](.Doc/架构介绍与开发指南.md)** ： **重要**，开发必看。若你希望为bsp或module增添新的模块，组装新的app，请参照此文档的编码和命名规范进行。阅读该文档有助于理解并写出和框架代码风格一致的程序。内含该项目的**文件树**，以框架的工作目录。
- [让VSCode成为更称手的IDE](.Doc/让VSCode成为更称手的IDE.md) ：安装好用的插件，对编辑器进行个性化配置，提升开发效率
### 阅读代码
框架中的三层结构都有详尽的注释帮助阅读和二次开发，三个抽象层都有各自总览的说明文档，而每个bsp/module/app都配有对应的个性化说明文档，提供了接口说明和改进或进一步开发的建议。
建议以自上而下的方式阅读代码，app-》module-》bsp，为此我们还提供了框架的说明视频，分别讲解每个抽象层和总体的设计思路，介绍了bsp的依赖HAL，还有一些杂碎的开发相关知识：[basic_framework教程](https://space.bilibili.com/522795884/channel/collectiondetail)
### 运行单个bsp/module测试
**每个bsp和module的文件夹中都有各自的说明文档**，测试用例和使用方法都在文档中给出，非常方便。
想要进行硬件查错、连线检查或测试，如测试开发板功能（外设）是否完好，电机电调能否正常使用，或给新人提供教学，只要：
1. 将main.c中的`RoboInit()`函数删除，然后包含`bsp_init.h`头文件，在原`RoboInit()`的位置添加一个`BSPInit()`
2. 包含用于测试的bsp或module头文件，根据说明文档的用例初始化对应的支持包或模块即可。
3. 同时可以将实时系统的初始化注释或删除，在`main.c`的`while(1)`主循环中进行测试，也可以使用`bsp_tim.h`提供的定时任务。
4. 编译，下载，运行，调试。
我们为机械和视觉的同学能方便测试硬件模块的好坏，设计了一套通过串口和遥控器控制的**硬件功能测试程序**，使得其他技术组成员可以操作这个”黑箱“，在没有电控组成员的时候也不会卡住其他队员的进度。
### VSCode集成工具
我们在`.vscode`下提供了编写好的一些任务，包括编译，烧录，启动RTT终端（LOG），启动Ozone调试等。有些功能需要配置vscode的插件设置或将一些可执行文件加入环境变量，这些步骤已经在[VSCode+Ozone使用方法.md](.Doc/VSCode+Ozone使用方法.md)中给出；`launch.json`里包含了最常见的四种调试任务：使用jlink-server/openocd，启动或附加调试。
### for pro-user
Makefile提供了脚本化的Makefile.upgrade，使用后者可以获取更好的开发体验。
可以自行添加需要的编译优化，进行更高级别的定制。
ST官方现在将HAL放入github维护。想要获取最新的支持，可以自行下载，加入本项目编译。
若希望纯开源使用，可以自定义openocd调试和烧录选项，参考根目录的openocd_dap.cfg和openocd_jlink.cfg。
若希望自己编译特定版本的cmsis-dsp或cmsis-os，请前往官方的github仓库下载，将构建规则加入makefile。
我们还增加了CMakeLists.txt以融入更现代化的构建系统，若你希望使用cmake，相信你有能力配置相关的开发环境。可以参考我们的[***powerful_framework***](https://gitee.com/hnuyuelurm/powerful_framework).
如果实时系统任务需要的栈空间不够，请在CubeMX初始化配置中增大任务栈。一些freertos支持的高级功能请自行在配置页开启宏定义后重新生成。
...
### DISABLE 保护
调用 `DMMotorSetMode(DM_CMD_DISABLE_MODE, motor)` 时：
1. 先发送零速指令 (`DMMotorSendVelocity(can, 0.0f)`) 让电机停止转动
2. 清理 angle_PID 全部状态 (积分/微分/误差)
3. 清零 pid_ref
4. 发送失能 CAN 命令 (0xFD)
防止切回 ENABLE 时因 PID 残余积分项或位置跳变导致的突变。
---
## 八、模块层
## 后续计划
- `.Doc/TODO.md`中列举了一些可能的功能增强和优化。
- 将所有配置移动到一处，方便修改（统一放入`app/robot_def.h`中？）
- 为三个层级都增加入门级培训教程，可以单独运行各个模块以方便上手。
- 优化pub-sub消息机制的性能，同时将app的任务尽可能修改为**状态机+事件驱动**的回调机制。
- 使用Qt或命令行为机器人配置（主要是robot_def和各个任务中的module初始化配置）编写UI界面，实现无代码机器人部署。
- 为框架编写ROSdriver，通过usb连接到上位机（NUC），合并视觉/算法和电控的工作流。
| 分类 | 模块 | 功能 |
|------|------|------|
| **电机** | `motor/DMmotor` | 达妙电机驱动 (MIT/PosVel/Velocity 三模式) |
| | `motor/motor_task` | 电机控制统一调度 (`DMMotorControl`) |
| **姿态** | `imu/ins_task` | 惯导解算 (四元数 EKF, 返回 INS_t 指针) |
| | `imu/BMI088driver` | BMI088 SPI 读取 |
| | `imu/BMI088Middleware` | BMI088 数据换算 |
| **算法** | `algorithm/controller` | PID 控制器 (梯形积分/变速积分/微分先行/输出滤波) |
| | `algorithm/kalman_filter` | 卡尔曼滤波器 (自适应) |
| | `algorithm/QuaternionEKF` | 四元数 EKF (陀螺零偏估计/卡方检验) |
| | `algorithm/crc8/crc16` | CRC 校验 |
| | `algorithm/user_lib` | 数学/工具函数 |
| **通信** | `master_machine` | 上位机通信 (0xA5/0xA6/0xA7/0xA8 协议, 接收+解析) |
| | `timachine/board_comm` | 板间通信 |
| | `message_center` | Pub-Sub 消息中间件 (链表, FIFO) |
| **系统** | `daemon` | 看门狗 (重载计数+离线回调) |
| | `alarm/buzzer` | 蜂鸣器 |
| | `vofa` | VoFA+ 调试输出 |
---
## 九、BSP 驱动层
实例化注册模式 (不注册不初始化)：
## 致谢
| 模块 | 功能 |
|------|------|
| `bsp_dwt` | DWT 周期计数器 (μs/ms 延时, 时间戳) |
| `bsp_can` | FDCAN 收发 (FIFO+中断) |
| `bsp_usart` | 串口收发 (DMA+环形缓冲区) |
| `bsp_spi` | SPI 通信 |
| `bsp_log` | SEGGER RTT + 串口日志 |
| `bsp_pwm` | PWM 输出 |
本框架设计参考了哈尔滨工业大学（深圳）南工骁鹰🦅战队的EC_framework以及RoboMaster官方的RoboRTS-firmware🤖。姿态解算改进自哈尔滨工程大学创梦之翼🛩️的四元数EKF姿态解算。裁判系统数据解析移植了深圳大学RoboPilot2021年电控英雄开源代码。
---
感谢2022-2023赛季跃鹿战队电控组参与新框架测试和开发的队员们，包括设计出机器人平台的机械组队员，还有一起联调的视觉组队员，以及负责拍摄、记录、宣传的运营组成员。
## 十、数据流
```
┌──────────────┐
│ 上位机 │
│ (USART10) │
└──────┬───────┘
│ 0xA5 / 0xA6 / 0xA7 / 0xA8
▼
┌────────────────────────┐
│ RobotCMD │
│ ProcessCommand() │ ← 模式切换/激光
│ VisionTracking() │ ← KF → vision PID → 角度增量
│ ManualMode() │ ← 直接角度参考
│ DebugMode() │ ← 实时 PID 调参
│ PubPushMessage() │
└───────┬───────┬────────┘
│ gimbal_cmd │ motor_pid / motor_enable
▼ ▼
┌────────────────────────────┐
│ Gimbal │
│ DMMotorSetRef() │
│ 电机使能/失能控制 │
│ 电机角 PID 在线更新 │
│ 反馈: gimbal_feed │ ← INS_t* 指针
└───────────┬────────────────┘
│ angle ref
▼
┌────────────────────────┐
│ DMMotor │ (1kHz Motor Task)
│ IMU angle → PID │
│ → Velocity mode CAN │
└────────────────────────┘
│
┌───────────┴────────────┐
│ DM-G6220 DM-J4310 │
│ (Yaw) (Pitch) │
└────────────────────────┘
```
---
## 十一、关键设计
1. **IMU 反馈替代编码器** — 云台 Yaw/Pitch 使用 INS_t 中的 YawTotalAngle/Pitch 和 Gyro[]
2. **速度模式控制** — MCU 跑角度 PID，电机内部处理速度闭环
3. **伪 Pub-Sub** — 应用间通过 message_center 解耦
4. **Daemon 看门狗** — 每个外设模块独立 DaemonInstance，离线自动回调
5. **BSP 实例化注册** — CAN/串口不注册不初始化
6. **KF + PID 级联** — 视觉误差经 KF 平滑 → vision PID → 角度增量 → 内环角度 PID
7. **模式切换重置** — 切换到自动模式时重置 KF 状态 (x=0, P=diag(5,5)) 和 PID 积分项
8. **串口命令控制** — 模式切换、激光控制均通过 0xA7 命令帧触发
9. **output_reverse** — PID 输出方向统一通过单标志位控制
10. **调试模式** — 上位机可实时调整视觉环和电机角度环 PID 参数，无需重新编译
11. **DISABLE 保护** — 电机失能时自动发送零速指令并清理 PID，防止切回使能时突变
---
## 十二、核心数据类型
```c
// INS 惯导实例 (ins_task.h)
typedef struct {
float q[4]; // 四元数
float Gyro[3]; // 角速度 (rad/s)
float Accel[3]; // 加速度 (m/s²)
float Roll, Pitch, Yaw;
float YawTotalAngle; // 多圈 yaw (degrees)
} INS_t;
// 云台控制指令 (robot_def.h)
typedef struct {
float yaw; // yaw 目标角度 (degrees)
float pitch; // pitch 目标角度 (degrees)
} Gimbal_Ctrl_Cmd_s;
// 上位机数据 (master_machine.h)
typedef struct {
float error_x, error_y; // 自动模式: 归一化误差
uint8_t auto_updated;
uint8_t zero_force; // 手动模式: 零力标志
float yaw_ref, pitch_ref; // 手动模式: 角度参考
uint8_t manual_updated;
// 调试模式数据
float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
float vision_pitch_Kp, vision_pitch_Ki, vision_pitch_Kd;
float motor_yaw_Kp, motor_yaw_Ki, motor_yaw_Kd;
float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
uint8_t debug_updated;
uint8_t cmd, cmd_pending; // 命令
float frequency;
} VisionData_t;
```
---
## 十三、项目规模
| 类别 | 数量 |
|------|------|
| C 源文件 | ~96 |
| 应用层模块 | 3 (robot, robot_cmd, gimbal) |
| 模块层子模块 | 12 |
| BSP 驱动模块 | 8 |
| FreeRTOS 任务 | 4 |
| Pub-Sub 话题 | 4 |
| 最高控制频率 | 1kHz |
| CPU 频率 | 480MHz |
| FLASH 使用 | ~132 KB / 1 MB (13%) |
| DTCMRAM 使用 | ~44 KB / 128 KB (34%) |
application/cmd/robot_cmd.c
+77 -5
View File
@@ -13,7 +13,7 @@
#include "bsp_log.h"
/* ---- 模式 ---- */
static Vision_Mode_e current_mode = VISION_MODE_MANUAL;
static Vision_Mode_e current_mode = VISION_MODE_DEBUG;
/* ---- 视觉追踪 PID (外环) ---- */
static PIDInstance vision_yaw_pid;
@@ -32,6 +32,8 @@ static float gimbal_pitch_ref;
/* ---- pub-sub ---- */
static Publisher_t *gimbal_cmd_pub;
static Subscriber_t *gimbal_feed_sub;
static Publisher_t *motor_enable_pub;
static Publisher_t *motor_pid_pub;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;
static Gimbal_Upload_Data_s gimbal_fetch_data;
@@ -91,6 +93,14 @@ static void SwitchToAutoMode(void)
vision_data.manual_updated = 0;
VisionPIDReset();
/* 从调试模式切回时重新使能电机 */
if (current_mode == VISION_MODE_DEBUG)
{
Motor_Enable_s msg = { .enable = 1 };
PubPushMessage(motor_enable_pub, &msg);
}
current_mode = VISION_MODE_AUTO;
LOGINFO("[CMD] switched to AUTO mode");
}
@@ -104,10 +114,32 @@ static void SwitchToManualMode(void)
gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch;
vision_data.auto_updated = 0;
/* 从调试模式切回时重新使能电机 */
if (current_mode == VISION_MODE_DEBUG)
{
Motor_Enable_s msg = { .enable = 1 };
PubPushMessage(motor_enable_pub, &msg);
}
current_mode = VISION_MODE_MANUAL;
LOGINFO("[CMD] switched to MANUAL mode");
}
static void SwitchToDebugMode(void)
{
if (current_mode == VISION_MODE_DEBUG)
return;
current_mode = VISION_MODE_DEBUG;
/* 失能 yaw 和 pitch 电机 */
Motor_Enable_s msg = { .enable = 0 };
PubPushMessage(motor_enable_pub, &msg);
LOGINFO("[CMD] switched to DEBUG mode, motors disabled");
}
/* ---- 命令处理 ---- */
static void ProcessCommand(void)
{
@@ -124,6 +156,9 @@ static void ProcessCommand(void)
case VISION_CMD_SWITCH_MANUAL:
SwitchToManualMode();
break;
case VISION_CMD_SWITCH_DEBUG:
SwitchToDebugMode();
break;
case VISION_CMD_LASER_ON:
laser_on();
break;
@@ -170,6 +205,8 @@ void RobotCMDInit()
{
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
motor_enable_pub = PubRegister("motor_enable", sizeof(Motor_Enable_s));
motor_pid_pub = PubRegister("motor_pid", sizeof(Motor_PID_Tune_s));
static PID_Init_Config_s yaw_vpid_config = {
.Kp = 1.5f, .Ki = 3.0f, .Kd = 0.07f,
@@ -196,7 +233,7 @@ void RobotCMDInit()
}
/* ---- 自动 / 视觉追踪模式 ---- */
static void VisionTrackingSet()
static void VisionTracking()
{
if (vision_data.auto_updated)
{
@@ -227,7 +264,7 @@ static void VisionTrackingSet()
}
/* ---- 手动模式: 上位机角度参考 ---- */
static void ManualModeSet()
static void ManualMode()
{
if (vision_data.manual_updated)
{
@@ -245,6 +282,39 @@ static void ManualModeSet()
gimbal_cmd_send.pitch = gimbal_pitch_ref;
}
/* ---- 调试模式: 上位机下发 PID 参数调参 ---- */
static void DebugMode()
{
if (vision_data.debug_updated)
{
vision_data.debug_updated = 0;
/* 直接设置视觉环 PID 参数 */
vision_yaw_pid.Kp = vision_data.vision_yaw_Kp;
vision_yaw_pid.Ki = vision_data.vision_yaw_Ki;
vision_yaw_pid.Kd = vision_data.vision_yaw_Kd;
vision_pitch_pid.Kp = vision_data.vision_pitch_Kp;
vision_pitch_pid.Ki = vision_data.vision_pitch_Ki;
vision_pitch_pid.Kd = vision_data.vision_pitch_Kd;
/* 发布电机角度环 PID 参数到 gimbal */
Motor_PID_Tune_s tune = {
.yaw_Kp = vision_data.motor_yaw_Kp,
.yaw_Ki = vision_data.motor_yaw_Ki,
.yaw_Kd = vision_data.motor_yaw_Kd,
.pitch_Kp = vision_data.motor_pitch_Kp,
.pitch_Ki = vision_data.motor_pitch_Ki,
.pitch_Kd = vision_data.motor_pitch_Kd,
};
PubPushMessage(motor_pid_pub, &tune);
LOGINFO("[CMD] PID tuned: v_yaw(%.2f,%.2f,%.2f) v_pitch(%.2f,%.2f,%.2f)",
vision_data.vision_yaw_Kp, vision_data.vision_yaw_Ki, vision_data.vision_yaw_Kd,
vision_data.vision_pitch_Kp, vision_data.vision_pitch_Ki, vision_data.vision_pitch_Kd);
}
}
void RobotCMDTask()
{
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
@@ -252,9 +322,11 @@ void RobotCMDTask()
ProcessCommand();
if (current_mode == VISION_MODE_AUTO)
VisionTrackingSet();
VisionTracking();
else if (current_mode == VISION_MODE_DEBUG)
DebugMode();
else
ManualModeSet();
ManualMode();
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
}
application/gimbal/gimbal.c
+35 -1
View File
@@ -9,6 +9,8 @@ static INS_t *gimbal_IMU_data;
static Publisher_t *gimbal_pub;
static Subscriber_t *gimbal_sub;
static Subscriber_t *motor_enable_sub;
static Subscriber_t *motor_pid_sub;
static Gimbal_Upload_Data_s gimbal_feedback_data;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;
@@ -21,6 +23,8 @@ void GimbalInit()
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
motor_enable_sub = SubRegister("motor_enable", sizeof(Motor_Enable_s));
motor_pid_sub = SubRegister("motor_pid", sizeof(Motor_PID_Tune_s));
/* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
DM_Motor_Init_Config_s yaw_config = {
@@ -78,6 +82,8 @@ void GimbalInit()
.mit_kp = 0, .mit_kd = 0,
};
pitch_motor = DMMotorInit(&pitch_config);
gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data;
}
void GimbalTask()
@@ -87,6 +93,34 @@ void GimbalTask()
DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data;
/* 电机使能/失能控制 */
static Motor_Enable_s enable_msg;
if (SubGetMessage(motor_enable_sub, &enable_msg))
{
if (enable_msg.enable)
{
DMMotorSetMode(DM_CMD_ENABLE_MODE, yaw_motor);
DMMotorSetMode(DM_CMD_ENABLE_MODE, pitch_motor);
}
else
{
DMMotorSetMode(DM_CMD_DISABLE_MODE, yaw_motor);
DMMotorSetMode(DM_CMD_DISABLE_MODE, pitch_motor);
}
}
/* 电机角度环 PID 调参 */
static Motor_PID_Tune_s pid_tune;
if (SubGetMessage(motor_pid_sub, &pid_tune))
{
yaw_motor->motor_controller.angle_PID.Kp = pid_tune.yaw_Kp;
yaw_motor->motor_controller.angle_PID.Ki = pid_tune.yaw_Ki;
yaw_motor->motor_controller.angle_PID.Kd = pid_tune.yaw_Kd;
pitch_motor->motor_controller.angle_PID.Kp = pid_tune.pitch_Kp;
pitch_motor->motor_controller.angle_PID.Ki = pid_tune.pitch_Ki;
pitch_motor->motor_controller.angle_PID.Kd = pid_tune.pitch_Kd;
}
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}
application/robot_def.h
+13
View File
@@ -47,6 +47,19 @@ typedef struct
uint8_t gimbal_power;
} Gimbal_Upload_Data_s;
/* ---- CMD 发布给 gimbal 的电机使能指令 ---- */
typedef struct
{
uint8_t enable; // 0=失能, 1=使能
} Motor_Enable_s;
/* ---- CMD 发布给 gimbal 的电机角度环 PID 参数 ---- */
typedef struct
{
float yaw_Kp, yaw_Ki, yaw_Kd;
float pitch_Kp, pitch_Ki, pitch_Kd;
} Motor_PID_Tune_s;
#pragma pack()
#endif // !ROBOT_DEF_H
docs/host_comm_guide.md
+244
View File
@@ -0,0 +1,244 @@
# 上位机通信协议指南
## 串口配置
| 参数 | 值 |
|---|---|
| 波特率 | 115200 |
| 数据位 | 8 |
| 停止位 | 1 |
| 校验位 | None |
## 帧格式总览
所有帧通过帧头 (SOF) 区分类型，checksum = 帧头到数据末尾前一字节的所有字节累加和取低 8 位。
| 帧头 | 长度 | 用途 |
|---|---|---|
| 0xA5 | 10 | 自动/视觉追踪数据 |
| 0xA6 | 11 | 手动控制数据 |
| 0xA7 | 3 | 命令 |
| 0xA8 | 50 | 调试调参数据 |
---
## 1. 自动/视觉追踪数据帧 0xA5 (10 字节)
**用途**: 视觉识别完成后持续发送跟踪误差。
```
[SOF:0xA5][error_x:4][error_y:4][checksum:1]
```
| 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA5 |
| 1 | 4 | float | 归一化 X 轴误差 |
| 5 | 4 | float | 归一化 Y 轴误差 |
| 9 | 1 | uint8 | checksum |
**Python 写入示例**:
```python
import struct
def send_auto_tracking(ser, error_x: float, error_y: float):
data = bytearray()
data.append(0xA5)
data.extend(struct.pack('<ff', error_x, error_y))
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 2. 手动控制数据帧 0xA6 (11 字节)
**用途**: 手动模式下发送角度目标值。
```
[SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
```
| 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA6 |
| 1 | 1 | uint8 | 零力标志 (0=正常, 1=零力) |
| 2 | 4 | float | Yaw 角度参考 (degrees) |
| 6 | 4 | float | Pitch 角度参考 (degrees) |
| 10 | 1 | uint8 | checksum |
**Python 写入示例**:
```python
def send_manual_control(ser, yaw_deg: float, pitch_deg: float, zero_force: int = 0):
data = bytearray()
data.append(0xA6)
data.append(0 if zero_force else 0)
data.extend(struct.pack('<ff', yaw_deg, pitch_deg))
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 3. 命令帧 0xA7 (3 字节)
**用途**: 切换模式和控制激光。
```
[SOF:0xA7][cmd:1][checksum:1]
```
| cmd 值 | 含义 |
|---|---|
| 0x01 | 切换到自动/视觉追踪模式 |
| 0x02 | 切换到手动模式 |
| 0x05 | 切换到调试模式 (失能所有电机) |
| 0x03 | 激光开 |
| 0x04 | 激光关 |
**Python 写入示例**:
```python
CMD_AUTO = 0x01
CMD_MANUAL = 0x02
CMD_LASER_ON = 0x03
CMD_LASER_OFF = 0x04
CMD_DEBUG = 0x05
def send_command(ser, cmd: int):
data = bytearray([0xA7, cmd])
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 4. 调试调参帧 0xA8 (50 字节)
**用途**: 在调试模式下发送 PID 参数进行调参。发送后固件会立即生效。
```
[SOF:0xA8]
[vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
[vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
[motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
[motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4]
[checksum:1]
```
| 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA8 |
| 1 | 4 | float | 视觉环 Yaw Kp |
| 5 | 4 | float | 视觉环 Yaw Ki |
| 9 | 4 | float | 视觉环 Yaw Kd |
| 13 | 4 | float | 视觉环 Pitch Kp |
| 17 | 4 | float | 视觉环 Pitch Ki |
| 21 | 4 | float | 视觉环 Pitch Kd |
| 25 | 4 | float | 电机角度环 Yaw Kp |
| 29 | 4 | float | 电机角度环 Yaw Ki |
| 33 | 4 | float | 电机角度环 Yaw Kd |
| 37 | 4 | float | 电机角度环 Pitch Kp |
| 41 | 4 | float | 电机角度环 Pitch Ki |
| 45 | 4 | float | 电机角度环 Pitch Kd |
| 49 | 1 | uint8 | checksum |
**Python 写入示例**:
```python
def send_debug_tune(ser,
v_yaw_kp, v_yaw_ki, v_yaw_kd,
v_pitch_kp, v_pitch_ki, v_pitch_kd,
m_yaw_kp, m_yaw_ki, m_yaw_kd,
m_pitch_kp, m_pitch_ki, m_pitch_kd):
data = bytearray()
data.append(0xA8)
data.extend(struct.pack('<fff', v_yaw_kp, v_yaw_ki, v_yaw_kd))
data.extend(struct.pack('<fff', v_pitch_kp, v_pitch_ki, v_pitch_kd))
data.extend(struct.pack('<fff', m_yaw_kp, m_yaw_ki, m_yaw_kd))
data.extend(struct.pack('<fff', m_pitch_kp, m_pitch_ki, m_pitch_kd))
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 调试流程
1. 上位机发送命令帧 (0xA7, cmd=0x05) 切换到调试模式
2. 固件收到后失能 yaw 和 pitch 电机，日志指示进入调试模式
3. 通过 0xA8 帧写入 PID 参数，参数会**实时生效**：
- 视觉环 PID → 直接写入选中的视觉 PID 实例
- 电机角度环 PID → 通过话题推送到 gimbal 更新电机角度 PID
4. 调试完毕后，发送命令帧 (0xA7, cmd=0x02 手动 或 0x01 自动) 切出调试模式，电机会自动使能
---
## Tkinter 界面建议
```python
import tkinter as tk
import tkinter.ttk as ttk
import serial
import struct
import threading
class TunerApp:
def __init__(self, root):
self.ser = serial.Serial('COM3', 115200, timeout=0.01)
# 视觉环 PID
self.vision_frame = ttk.LabelFrame(root, text="视觉环 PID")
self.v_yaw = self._make_pid_row(self.vision_frame, "Yaw", 0, (1.5, 3.0, 0.07))
self.v_pitch = self._make_pid_row(self.vision_frame, "Pitch", 1, (1.0, 2.0, 0.05))
# 电机角度环 PID
self.motor_frame = ttk.LabelFrame(root, text="电机角度环 PID")
self.m_yaw = self._make_pid_row(self.motor_frame, "Yaw", 2, (30, 80, 0.2))
self.m_pitch = self._make_pid_row(self.motor_frame, "Pitch", 3, (20, 50, 0.1))
ttk.Button(root, text="发送调参", command=self.send_tune).pack()
def _make_pid_row(self, parent, label, row, default):
frame = ttk.Frame(parent)
ttk.Label(frame, text=label).grid(row=0, column=0)
kp = tk.DoubleVar(value=default[0])
ki = tk.DoubleVar(value=default[1])
kd = tk.DoubleVar(value=default[2])
ttk.Entry(frame, textvariable=kp, width=8).grid(row=0, column=1)
ttk.Entry(frame, textvariable=ki, width=8).grid(row=0, column=2)
ttk.Entry(frame, textvariable=kd, width=8).grid(row=0, column=3)
frame.pack()
return (kp, ki, kd)
def send_tune(self):
def get(v): return v[0].get(), v[1].get(), v[2].get()
vy_kp, vy_ki, vy_kd = get(self.v_yaw)
vp_kp, vp_ki, vp_kd = get(self.v_pitch)
my_kp, my_ki, my_kd = get(self.m_yaw)
mp_kp, mp_ki, mp_kd = get(self.m_pitch)
send_debug_tune(self.ser,
vy_kp, vy_ki, vy_kd,
vp_kp, vp_ki, vp_kd,
my_kp, my_ki, my_kd,
mp_kp, mp_ki, mp_kd)
root = tk.Tk()
app = TunerApp(root)
root.mainloop()
```
## 校验和计算
```python
def calc_checksum(data: bytes) -> int:
return sum(data) & 0xFF # 所有字节累加取低 8 位
```
校验和覆盖范围：**帧头到数据末尾前一字节**的全部字节。接收端在收到数据后也应计算并比对校验和。
modules/master_machine/master_machine.c
+19
View File
@@ -42,6 +42,25 @@ static void DecodeVision()
memcpy(&vision_data.pitch_ref, buf + 6, 4);
vision_data.manual_updated = 1;
}
else if (sof == VISION_SOF_DEBUG && len == VISION_DEBUG_LEN)
{
if (calc_checksum(buf, VISION_DEBUG_LEN) != buf[VISION_DEBUG_LEN - 1])
return;
uint8_t *p = buf + 1;
memcpy(&vision_data.vision_yaw_Kp, p, 4); p += 4;
memcpy(&vision_data.vision_yaw_Ki, p, 4); p += 4;
memcpy(&vision_data.vision_yaw_Kd, p, 4); p += 4;
memcpy(&vision_data.vision_pitch_Kp, p, 4); p += 4;
memcpy(&vision_data.vision_pitch_Ki, p, 4); p += 4;
memcpy(&vision_data.vision_pitch_Kd, p, 4); p += 4;
memcpy(&vision_data.motor_yaw_Kp, p, 4); p += 4;
memcpy(&vision_data.motor_yaw_Ki, p, 4); p += 4;
memcpy(&vision_data.motor_yaw_Kd, p, 4); p += 4;
memcpy(&vision_data.motor_pitch_Kp, p, 4); p += 4;
memcpy(&vision_data.motor_pitch_Ki, p, 4); p += 4;
memcpy(&vision_data.motor_pitch_Kd, p, 4);
vision_data.debug_updated = 1;
}
else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN)
{
if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1])
modules/master_machine/master_machine.h
+20 -2
View File
@@ -9,7 +9,7 @@
#include <string.h>
/*
* 上位机串口协议 — 三种帧类型, 通过帧头区分
* 上位机串口协议 — 四种帧类型, 通过帧头区分
*
* ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ----
* [SOF:0xA5][error_x:4][error_y:4][checksum:1]
@@ -22,7 +22,14 @@
*
* ---- 命令帧 0xA7 (3 字节) ----
* [SOF:0xA7][cmd:1][checksum:1]
* cmd: 0x01=切换到自动模式, 0x02=切换到手动模式, 0x03=激光开, 0x04=激光关
* cmd: 0x01=自动模式, 0x02=手动模式, 0x03=激光开, 0x04=激光关, 0x05=调试模式
*
* ---- 调试调参帧 0xA8 (50 字节) ----
* [SOF:0xA8][vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
* [vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
* [motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
* [motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4][checksum:1]
* 全部 float, 依次为视觉环 yaw/pitch 和电机角度环 yaw/pitch 的 PID 参数
*
* checksum: 帧头到数据末尾前一字节累加和取低8位
*/
@@ -30,22 +37,26 @@
#define VISION_SOF_AUTO 0xA5
#define VISION_SOF_MANUAL 0xA6
#define VISION_SOF_CMD 0xA7
#define VISION_SOF_DEBUG 0xA8
#define VISION_AUTO_LEN 10
#define VISION_MANUAL_LEN 11
#define VISION_CMD_LEN 3
#define VISION_DEBUG_LEN 50
/* 命令码 */
#define VISION_CMD_SWITCH_AUTO 0x01
#define VISION_CMD_SWITCH_MANUAL 0x02
#define VISION_CMD_LASER_ON 0x03
#define VISION_CMD_LASER_OFF 0x04
#define VISION_CMD_SWITCH_DEBUG 0x05
/* 工作模式 */
typedef enum
{
VISION_MODE_AUTO = 0, // 自动/视觉追踪
VISION_MODE_MANUAL = 1, // 手动
VISION_MODE_DEBUG = 2, // 调试
} Vision_Mode_e;
typedef struct
@@ -61,6 +72,13 @@ typedef struct
float pitch_ref;
uint8_t manual_updated; // 收到新的手动数据帧标志
/* 调试模式数据 */
float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
float vision_pitch_Kp, vision_pitch_Ki, vision_pitch_Kd;
float motor_yaw_Kp, motor_yaw_Ki, motor_yaw_Kd;
float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
uint8_t debug_updated;
/* 命令 */
uint8_t cmd; // 最新命令码
uint8_t cmd_pending; // 有待处理命令标志
modules/motor/DMmotor/dmmotor.c
+22 -8
View File
@@ -91,12 +91,29 @@ void DMMotorSendVelocity(CANInstance *can, float vel)
/* ---- Motor commands (enable/disable/zero/clear error) ---- */
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
void DMMotorSetMode(DM_Motor_Mode_e cmd, DMMotorInstance *motor)
{
if (cmd == DM_CMD_DISABLE_MODE)
{
/* 先发送零速度指令让电机停止,防止切回 ENABLE 时突变 */
DMMotorSendVelocity(motor->motor_can_instance, 0.0f);
/* 清理 PID 状态 */
motor->motor_controller.angle_PID.Iout = 0;
motor->motor_controller.angle_PID.ITerm = 0;
motor->motor_controller.angle_PID.Output = 0;
motor->motor_controller.angle_PID.Err = 0;
motor->motor_controller.angle_PID.Last_Err = 0;
motor->motor_controller.angle_PID.Dout = 0;
motor->motor_controller.angle_PID.Last_Dout = 0;
motor->motor_controller.pid_ref = 0;
}
memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
motor->motor_can_instance->txconf.Identifier = motor->motor_can_instance->tx_id;
CANTransmit(motor->motor_can_instance, 1);
motor->motor_mode = cmd;
}
/* ---- Feedback decode: CAN RX interrupt callback ---- */
@@ -175,7 +192,7 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
//motor->motor_daemon = DaemonRegister(&conf);
/* enable motor */
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DMMotorSetMode(DM_CMD_ENABLE_MODE, motor);
dm_motor_instance[idx++] = motor;
return motor;
@@ -188,11 +205,6 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref)
motor->motor_controller.pid_ref = ref;
}
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
{
motor->ctrl_mode = mode;
}
/* ---- Main control: called from Motor Task at 1kHz ---- */
/* 硬编码: IMU 角度反馈 → 角度 PID → 速度指令 → 电机速度模式 */
@@ -201,6 +213,8 @@ void DMMotorControl(void)
for (uint8_t i = 0; i < idx; i++)
{
DMMotorInstance *motor = dm_motor_instance[i];
if(motor->motor_mode == DM_CMD_DISABLE_MODE)
continue;
CANInstance *can = motor->motor_can_instance;
/* handle motor error states */
@@ -208,7 +222,7 @@ void DMMotorControl(void)
{
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
DWT_Delay(0.001);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DMMotorSetMode(DM_CMD_ENABLE_MODE, motor);
continue;
}
modules/motor/DMmotor/dmmotor.h
+15 -14
View File
@@ -21,13 +21,23 @@
#define DM_KD_MIN 0.0f
#define DM_KD_MAX 5.0f
/* DM motor control modes */
/* DM motor command codes */
typedef enum
{
DM_CMD_ENABLE_MODE = 0xFC, // enable motor
DM_CMD_DISABLE_MODE = 0xFD, // disable motor
DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero
DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error
} DM_Motor_Mode_e;
/* DM motor control modes */
typedef enum
{
DM_MODE_MIT = 0, // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
DM_MODE_POS_VEL = 1, // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
DM_MODE_VELOCITY = 2, // Velocity mode: CAN ID + 0x200, float velocity only
} DM_Motor_Mode_e;
} DM_Motor_Ctrl_Mode_e;
/* DM motor measurement data (parsed from feedback CAN frame) */
typedef struct
@@ -53,7 +63,8 @@ typedef struct
CANInstance *motor_can_instance; // CAN communication instance
DaemonInstance *motor_daemon; // daemon watchdog
Output_Reverse_Flag_e output_reverse; // output reverse flag
DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
DM_Motor_Ctrl_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
DM_Motor_Mode_e motor_mode; // last command mode (enable/disable/zero/clear error)
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
@@ -63,27 +74,17 @@ typedef struct
typedef struct
{
Motor_Init_Config_s motor_config; // generic motor PID and setting config
DM_Motor_Mode_e ctrl_mode; // default control mode
DM_Motor_Ctrl_Mode_e ctrl_mode; // default control mode
float mit_kp; // MIT mode KP (only for MIT mode)
float mit_kd; // MIT mode KD (only for MIT mode)
} DM_Motor_Init_Config_s;
/* DM motor command codes */
typedef enum
{
DM_CMD_MOTOR_MODE = 0xFC, // enable motor
DM_CMD_RESET_MODE = 0xFD, // disable motor
DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero
DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error
} DMMotor_Mode_e;
/* ---- API ---- */
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
void DMMotorSetMode(DM_Motor_Mode_e mode, DMMotorInstance *motor);
/**
* @brief Main control function called from Motor Task at 1kHz.