From 9b8e204ff77d715e92186d5ba2b73f79047035b6 Mon Sep 17 00:00:00 2001 From: SkyLi Date: Sun, 31 May 2026 03:49:41 +0800 Subject: [PATCH] feat: add DEBUG mode with host-side PID tuning, motor enable/disable protection - 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 --- ARCHITECTURE.md | 310 ------------ README.md | 605 ++++++++++++------------ application/cmd/robot_cmd.c | 82 +++- application/gimbal/gimbal.c | 36 +- application/robot_def.h | 13 + docs/host_comm_guide.md | 244 ++++++++++ modules/master_machine/master_machine.c | 19 + modules/master_machine/master_machine.h | 22 +- modules/motor/DMmotor/dmmotor.c | 30 +- modules/motor/DMmotor/dmmotor.h | 29 +- 10 files changed, 747 insertions(+), 643 deletions(-) delete mode 100644 ARCHITECTURE.md create mode 100644 docs/host_comm_guide.md diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md deleted file mode 100644 index 8419fc6..0000000 --- a/ARCHITECTURE.md +++ /dev/null @@ -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; -``` diff --git a/README.md b/README.md index 4cf83f6..12e9053 100644 --- a/README.md +++ b/README.md @@ -1,356 +1,355 @@ -# 电赛-云台-dmmc02 - -基于达妙MC02的电赛云台开发板代码 - -带卡尔曼滤波版本 - -powered by ggllxx - -可用于江南大学2025电赛E题作品开源 - -以下是原框架描述： - ------- - - -# YueLu2022/2023 EC basic_framework-dev -湖南大学RoboMaster跃鹿战队2022-2023电控通用嵌入式框架。 - -
- -

- - - -

-

- - -

-

Best RoboMaster embedded EC open-source code ever?

-
- - - -> ***也许不是最好的？但一定是最完整最详细最适合上手的电控开源！*** - - - -***==别忘了仓库根目录下的`.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，忍受没有补全和高亮的代码了！在现代化开发工具支持下，将**极大提高电控开发效率和调试效率**。硬件模块测试和整车联调从未**如此简单方便**！ - -> 用软件开发的思想设计嵌入式系统是一种降维打击 -> ->

—— 沃兹基·烁德

- -### 效果展示 - -![](.assets/allrobot.jpg) - -
搭载basic_framework的机器人阵容
- -实战展示： - -1. 400HP双云台哨兵30m/s弹速**对抗**2台200HP 15m/s弹速步兵，120w底盘功率 -2. 100HP步兵，120w功率，15m/s弹速**对射**，限制射频0.5发/s - -![sentry_infantry12](.assets/sentry_infantry12.gif) - -
展示中的视觉识别与预测算法是基于rm_vision打造的
- -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 传感器 | --- +## 二、分层架构 - -## 架构 - -总览。 - -### 软件栈 - -image-20230725153133419 - -在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 控制输出方向 │ +└──────────────────────────────────────────────────────┘ +``` -image-20230725152433502 +--- + +## 五、上位机串口协议 + +位于 `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 参数 -
建议浏览器打开SVG查看
- -​ +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%) | diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index d7583b3..3a38266 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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); } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5d9ddc8..cc99bb1 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -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); } diff --git a/application/robot_def.h b/application/robot_def.h index aa7e508..6a9ec91 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -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 diff --git a/docs/host_comm_guide.md b/docs/host_comm_guide.md new file mode 100644 index 0000000..58b25c0 --- /dev/null +++ b/docs/host_comm_guide.md @@ -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(' int: + return sum(data) & 0xFF # 所有字节累加取低 8 位 +``` + +校验和覆盖范围：**帧头到数据末尾前一字节**的全部字节。接收端在收到数据后也应计算并比对校验和。 diff --git a/modules/master_machine/master_machine.c b/modules/master_machine/master_machine.c index 63e8595..9a548b0 100644 --- a/modules/master_machine/master_machine.c +++ b/modules/master_machine/master_machine.c @@ -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]) diff --git a/modules/master_machine/master_machine.h b/modules/master_machine/master_machine.h index 9a230d5..30443c0 100644 --- a/modules/master_machine/master_machine.h +++ b/modules/master_machine/master_machine.h @@ -9,7 +9,7 @@ #include /* - * 上位机串口协议 — 三种帧类型, 通过帧头区分 + * 上位机串口协议 — 四种帧类型, 通过帧头区分 * * ---- 自动/视觉追踪数据帧 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; // 有待处理命令标志 diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index c473307..425f71c 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -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; } diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index cfa2bac..2c09516 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -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.