实现完整串口指令协议：模式切换、手动控制、PID在线调参
- 新增 PIDClear() 仅清运行时状态，保留 Kp/Ki/Kd 等配置 - 命令帧 0xA7 (3B): 自动/手动/调试三模式切换，带模式状态追踪 - 手动控制帧 0xA6 (8B): 解析 motor_speed_base(int16) + angle_velocity(float) - 调试调参帧 0xA8 (38B): 在线更新全部9个 Kp/Ki/Kd 参数 - Command 解析纳入 Task_50ms (20Hz) 定时执行 - 添加通信协议文档 docs/通信协议.md
This commit is contained in:
@@ -0,0 +1,222 @@
|
|||||||
|
# 串口通信协议
|
||||||
|
|
||||||
|
## 概述
|
||||||
|
|
||||||
|
- **物理层**: UART2，引脚 TX=B15 / RX=B16
|
||||||
|
- **波特率**: 115200 bps
|
||||||
|
- **数据位**: 8，无校验，1 停止位（默认 UART 配置）
|
||||||
|
- **字节序**: 多字节数值均采用**小端序 (little-endian)**
|
||||||
|
- **校验方式**: 累加和校验（checksum = 帧内校验位之前所有字节之和，截断为 uint8_t）
|
||||||
|
- **软件 FIFO**: 16 字节环形缓冲区，UART RX 中断中写入，主循环 `command_task()` 中解析
|
||||||
|
|
||||||
|
## 工作模式
|
||||||
|
|
||||||
|
系统有三种工作模式，通过命令帧切换：
|
||||||
|
|
||||||
|
| 模式 | 枚举值 | 说明 | 使能的控制环 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| 自动模式 | `COMMAND_MODE_AUTO` (0) | 视觉追踪 / 全闭环 | 电机速度环 + 角度环 + 位置环 |
|
||||||
|
| 手动模式 | `COMMAND_MODE_MANUAL` (1) | 手动遥控 | 电机速度环 + 角度环 |
|
||||||
|
| 调试模式 | `COMMAND_MODE_DEBUG` (2) | PID 参数整定 | 无（所有控制环关闭） |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 数据帧定义
|
||||||
|
|
||||||
|
### 1. 命令帧 `0xA7`（3 字节）
|
||||||
|
|
||||||
|
切换工作模式。
|
||||||
|
|
||||||
|
```
|
||||||
|
┌────────┬───────┬──────────┐
|
||||||
|
│ SOF │ cmd │ checksum │
|
||||||
|
│ 0xA7 │ 1 B │ 1 B │
|
||||||
|
└────────┴───────┴──────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
| 偏移 | 长度 | 字段 | 类型 | 说明 |
|
||||||
|
|---|---|---|---|---|
|
||||||
|
| 0 | 1 | SOF | uint8_t | 帧头，固定值 `0xA7` |
|
||||||
|
| 1 | 1 | cmd | uint8_t | 命令码 |
|
||||||
|
| 2 | 1 | checksum | uint8_t | `(0xA7 + cmd) & 0xFF` |
|
||||||
|
|
||||||
|
**cmd 取值：**
|
||||||
|
|
||||||
|
| 值 | 功能 | 对应函数 |
|
||||||
|
|---|---|---|
|
||||||
|
| `0x01` | 切换到自动模式 | `SwitchToAutoMode()` |
|
||||||
|
| `0x02` | 切换到手动模式 | `SwitchToManualMode()` |
|
||||||
|
| `0x03` | 切换到调试模式 | `SwitchToDebugMode()` |
|
||||||
|
|
||||||
|
**示例（切换手动模式）：** `A7 02 A9`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### 2. 手动控制帧 `0xA6`（8 字节）
|
||||||
|
|
||||||
|
仅在**手动模式**下生效，用于实时控制底盘运动。
|
||||||
|
|
||||||
|
```
|
||||||
|
┌────────┬────────────────────┬─────────────────────┬──────────┐
|
||||||
|
│ SOF │ motor_speed_base │ angle_velocity │ checksum │
|
||||||
|
│ 0xA6 │ int16_t (2 B) │ float (4 B) │ 1 B │
|
||||||
|
└────────┴────────────────────┴─────────────────────┴──────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
| 偏移 | 长度 | 字段 | 类型 | 说明 |
|
||||||
|
|---|---|---|---|---|
|
||||||
|
| 0 | 1 | SOF | uint8_t | 帧头，固定值 `0xA6` |
|
||||||
|
| 1 | 2 | motor_speed_base | int16_t (LE) | 基础直行速度，范围 -50~+50 |
|
||||||
|
| 3 | 4 | angle_velocity | float (LE) | 角度速率，单位 °/s |
|
||||||
|
| 7 | 1 | checksum | uint8_t | `(0xA6 + 前 6 字节之和) & 0xFF` |
|
||||||
|
|
||||||
|
**控制算法：**
|
||||||
|
```
|
||||||
|
motor_speed_left_ref = motor_speed_base - speed_dif
|
||||||
|
motor_speed_right_ref = motor_speed_base + speed_dif
|
||||||
|
```
|
||||||
|
其中 `speed_dif` 由角度环 PID 根据 `yaw_ref` 与当前 `imu_angle.yaw` 计算得出。手动模式下 `yaw_ref` 以 `angle_velocity` 速率持续累加：
|
||||||
|
```
|
||||||
|
yaw_ref += angle_velocity * dt (dt = 0.02s)
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### 3. 调试调参帧 `0xA8`（38 字节）
|
||||||
|
|
||||||
|
仅在**调试模式**下生效，一次性更新所有 PID 参数（Kp / Ki / Kd）。
|
||||||
|
|
||||||
|
```
|
||||||
|
┌────────┬──────────┬──────────┬──────────┬──────────┬──────────┬──────────┬──────────┬──────────┬──────────┬──────────┐
|
||||||
|
│ SOF │ Kp_speed │ Ki_speed │ Kd_speed │ Kp_yaw │ Ki_yaw │ Kd_yaw │ Kp_pos │ Ki_pos │ Kd_pos │ checksum │
|
||||||
|
│ 0xA8 │ float(4) │ float(4) │ float(4) │ float(4) │ float(4) │ float(4) │ float(4) │ float(4) │ float(4) │ 1 B │
|
||||||
|
└────────┴──────────┴──────────┴──────────┴──────────┴──────────┴──────────┴──────────┴──────────┴──────────┴──────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
| 偏移 | 长度 | 字段 | 类型 | 说明 |
|
||||||
|
|---|---|---|---|---|
|
||||||
|
| 0 | 1 | SOF | uint8_t | 帧头，固定值 `0xA8` |
|
||||||
|
| 1 | 4 | Kp_speed | float (LE) | 电机速度环比例系数（左右共用） |
|
||||||
|
| 5 | 4 | Ki_speed | float (LE) | 电机速度环积分系数 |
|
||||||
|
| 9 | 4 | Kd_speed | float (LE) | 电机速度环微分系数 |
|
||||||
|
| 13 | 4 | Kp_yaw | float (LE) | 角度环比例系数 |
|
||||||
|
| 17 | 4 | Ki_yaw | float (LE) | 角度环积分系数 |
|
||||||
|
| 21 | 4 | Kd_yaw | float (LE) | 角度环微分系数 |
|
||||||
|
| 25 | 4 | Kp_position | float (LE) | 位置环比例系数 |
|
||||||
|
| 29 | 4 | Ki_position | float (LE) | 位置环积分系数 |
|
||||||
|
| 33 | 4 | Kd_position | float (LE) | 位置环微分系数 |
|
||||||
|
| 37 | 1 | checksum | uint8_t | `(0xA8 + 前 36 字节之和) & 0xFF` |
|
||||||
|
|
||||||
|
**各环默认参数：**
|
||||||
|
|
||||||
|
| 控制环 | Kp | Ki | Kd | 输出限幅 | 采样周期 | 优化环节 |
|
||||||
|
|---|---|---|---|---|---|---|
|
||||||
|
| 电机速度环（左/右） | 300.0 | 150.0 | 0.0 | ±3000 (MOTOR_MAX) | 10ms | — |
|
||||||
|
| 角度环 | 1.0 | 0.0 | 0.0 | ±25 (°/s) | 20ms | 偏航角误差修正 |
|
||||||
|
| 位置环 | 0.5 | 0.0 | 0.0 | ±90 (°/s) | 20ms | — |
|
||||||
|
|
||||||
|
> **注意**：调试模式下所有控制任务已关闭，电机停转。修改参数后需切换到自动或手动模式才能观察效果。修改的参数直接写入 PID 实例的 Kp/Ki/Kd 字段，不清除积分等运行时状态。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### 4. 角度速率前馈帧 `0xA9`（6 字节）— MCU → PC
|
||||||
|
|
||||||
|
由 MCU 经 DMA 主动发送，上报位置环计算的角度速率，供云台等设备做前馈控制。
|
||||||
|
|
||||||
|
```
|
||||||
|
┌────────┬─────────────────────┬──────────┐
|
||||||
|
│ SOF │ angle_velocity │ checksum │
|
||||||
|
│ 0xA9 │ float (4 B) │ 1 B │
|
||||||
|
└────────┴─────────────────────┴──────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
| 偏移 | 长度 | 字段 | 类型 | 说明 |
|
||||||
|
|---|---|---|---|---|
|
||||||
|
| 0 | 1 | SOF | uint8_t | 帧头，固定值 `0xA9` |
|
||||||
|
| 1 | 4 | angle_velocity | float (LE) | 角度速率，单位 °/s |
|
||||||
|
| 5 | 1 | checksum | uint8_t | `(0xA9 + 前 4 字节之和) & 0xFF` |
|
||||||
|
|
||||||
|
**发送触发条件**：位置环控制任务使能时，每 20ms（50Hz）在 `position_control_task()` 中调用 `command_send_angle_velocity()`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 通信流程
|
||||||
|
|
||||||
|
```
|
||||||
|
┌──────────┐ ┌──────────┐
|
||||||
|
│ PC │ ──── 0xA7 cmd ────────→ │ MSPM0 │
|
||||||
|
│ (上位机) │ ←─── 0xA9 feedback ──── │ (MCU) │
|
||||||
|
│ │ ──── 0xA6 manual ─────→ │ │
|
||||||
|
│ │ ──── 0xA8 debug ──────→ │ │
|
||||||
|
└──────────┘ └──────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
**典型操作流程：**
|
||||||
|
|
||||||
|
1. **上电默认**：自动模式，位置环+角度环+速度环全部使能
|
||||||
|
2. **切换到手动模式**：发送 `A7 02 A9`，然后周期发送 `0xA6` 控制帧驱动底盘
|
||||||
|
3. **PID 调参**：发送 `A7 03 AA` 进入调试模式（电机停转），发送 `0xA8` 调参帧更新参数，再发 `A7 01 A8` 或 `A7 02 A9` 切换回运行模式观察效果
|
||||||
|
|
||||||
|
## 模式切换副作用
|
||||||
|
|
||||||
|
每次切换模式时，系统会执行以下操作：
|
||||||
|
|
||||||
|
| 操作 | 自动模式 | 手动模式 | 调试模式 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| 设置 `current_mode` | AUTO (0) | MANUAL (1) | DEBUG (2) |
|
||||||
|
| 清除 PID 运行时状态 | 全部 4 个 PID | 速度环+角度环 | — |
|
||||||
|
| `yaw_ref` | = 当前 `imu_angle.yaw` | = 当前 `imu_angle.yaw` | — |
|
||||||
|
| `angle_velocity` | 归零 | 归零 | — |
|
||||||
|
| `motor_speed_ref` | 归零 | 归零 | — |
|
||||||
|
| `motor_speed_base` | = 10 (AUTO_MODE_BASE_SPEED) | 归零 | — |
|
||||||
|
| 电机 PWM | 归零 | 归零 | 归零 |
|
||||||
|
| 速度环使能 | ✅ | ✅ | ❌ |
|
||||||
|
| 角度环使能 | ✅ | ✅ | ❌ |
|
||||||
|
| 位置环使能 | ✅ | ❌ | ❌ |
|
||||||
|
| 蜂鸣器 | 100ms | 200ms | 300ms |
|
||||||
|
|
||||||
|
## C 语言帧构造示例
|
||||||
|
|
||||||
|
```c
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
// --- 构造 0xA7 命令帧 ---
|
||||||
|
void build_cmd_frame(uint8_t *buf, uint8_t cmd) {
|
||||||
|
buf[0] = 0xA7;
|
||||||
|
buf[1] = cmd;
|
||||||
|
buf[2] = (uint8_t)(0xA7 + cmd);
|
||||||
|
// 发送 buf, 长度 3
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- 构造 0xA6 手动控制帧 ---
|
||||||
|
void build_manual_frame(uint8_t *buf, int16_t speed, float yaw_rate) {
|
||||||
|
buf[0] = 0xA6;
|
||||||
|
memcpy(&buf[1], &speed, 2); // int16_t, 小端
|
||||||
|
memcpy(&buf[3], &yaw_rate, 4); // float, 小端
|
||||||
|
uint8_t sum = 0xA6;
|
||||||
|
for (int i = 1; i < 7; i++) sum += buf[i];
|
||||||
|
buf[7] = sum;
|
||||||
|
// 发送 buf, 长度 8
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- 构造 0xA8 调试调参帧 ---
|
||||||
|
void build_debug_frame(uint8_t *buf, float kp_s, float ki_s, float kd_s,
|
||||||
|
float kp_y, float ki_y, float kd_y,
|
||||||
|
float kp_p, float ki_p, float kd_p) {
|
||||||
|
buf[0] = 0xA8;
|
||||||
|
memcpy(&buf[1], &kp_s, 4);
|
||||||
|
memcpy(&buf[5], &ki_s, 4);
|
||||||
|
memcpy(&buf[9], &kd_s, 4);
|
||||||
|
memcpy(&buf[13], &kp_y, 4);
|
||||||
|
memcpy(&buf[17], &ki_y, 4);
|
||||||
|
memcpy(&buf[21], &kd_y, 4);
|
||||||
|
memcpy(&buf[25], &kp_p, 4);
|
||||||
|
memcpy(&buf[29], &ki_p, 4);
|
||||||
|
memcpy(&buf[33], &kd_p, 4);
|
||||||
|
uint8_t sum = 0xA8;
|
||||||
|
for (int i = 1; i < 37; i++) sum += buf[i];
|
||||||
|
buf[37] = sum;
|
||||||
|
// 发送 buf, 长度 38
|
||||||
|
}
|
||||||
|
```
|
||||||
+183
-18
@@ -5,11 +5,19 @@
|
|||||||
#include "ti/driverlib/dl_dma.h"
|
#include "ti/driverlib/dl_dma.h"
|
||||||
#include "zf_common_fifo.h"
|
#include "zf_common_fifo.h"
|
||||||
#include "Beep.h"
|
#include "Beep.h"
|
||||||
|
#include "Task.h"
|
||||||
|
#include "Motor.h"
|
||||||
|
#include "PID_Controller.h"
|
||||||
|
#include "IMU.h"
|
||||||
|
|
||||||
volatile fifo_struct command_fifo = {0};
|
volatile fifo_struct command_fifo = {0};
|
||||||
volatile uint8_t command_fifo_buffer[COMMAND_FIFO_BUFFER_SIZE] = {0};
|
volatile uint8_t command_fifo_buffer[COMMAND_FIFO_BUFFER_SIZE] = {0};
|
||||||
|
volatile Vision_Mode_e current_mode = COMMAND_MODE_DEBUG;
|
||||||
uint8_t command_sum(uint8_t* data,int len);
|
uint8_t command_sum(uint8_t* data,int len);
|
||||||
|
|
||||||
|
static void command_process_manual(uint8_t *data);
|
||||||
|
static void command_process_debug(uint8_t *data);
|
||||||
|
|
||||||
void command_callback(uint32 state, void *ptr)
|
void command_callback(uint32 state, void *ptr)
|
||||||
{
|
{
|
||||||
while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据
|
while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据
|
||||||
@@ -52,33 +60,73 @@ void command_init()
|
|||||||
void command_task()
|
void command_task()
|
||||||
{
|
{
|
||||||
static uint8_t state = 0;
|
static uint8_t state = 0;
|
||||||
static uint8_t cmd;
|
static uint8_t sof = 0;
|
||||||
|
static uint8_t payload_len = 0;
|
||||||
|
static uint8_t buf[37]; // 最大payload: 0xA8的37字节(9*4 float + 1 checksum 先读到buf)
|
||||||
|
static uint8_t idx = 0;
|
||||||
uint8_t byte;
|
uint8_t byte;
|
||||||
|
|
||||||
while(fifo_used((fifo_struct*)&command_fifo) > 0)
|
while(fifo_used((fifo_struct*)&command_fifo) > 0)
|
||||||
{
|
{
|
||||||
switch(state)
|
switch(state)
|
||||||
{
|
{
|
||||||
case 0: // 等待SOF 0xA7
|
case 0: // 等待SOF
|
||||||
fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
|
fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
|
||||||
if(byte == 0xA7) state = 1;
|
if(byte == COMMAND_SOF_MODE)
|
||||||
break;
|
|
||||||
case 1: // 读取cmd
|
|
||||||
fifo_read_element((fifo_struct*)&command_fifo, &cmd, FIFO_READ_AND_CLEAN);
|
|
||||||
state = 2;
|
|
||||||
break;
|
|
||||||
case 2: // 校验checksum = SOF + cmd
|
|
||||||
fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
|
|
||||||
if(byte == (uint8_t)(0xA7 + cmd))
|
|
||||||
{
|
{
|
||||||
switch(cmd)
|
sof = COMMAND_SOF_MODE;
|
||||||
{
|
payload_len = 2; // cmd(1) + checksum(1)
|
||||||
case 0x01: SwitchToAutoMode(); break;
|
idx = 0;
|
||||||
case 0x02: SwitchToManualMode(); break;
|
state = 1;
|
||||||
case 0x03: SwitchToDebugMode(); break;
|
}
|
||||||
}
|
else if(byte == COMMAND_SOF_MANUAL)
|
||||||
|
{
|
||||||
|
sof = COMMAND_SOF_MANUAL;
|
||||||
|
payload_len = 7; // motor_speed_base(2) + angle_velocity(4) + checksum(1)
|
||||||
|
idx = 0;
|
||||||
|
state = 1;
|
||||||
|
}
|
||||||
|
else if(byte == COMMAND_SOF_DEBUG)
|
||||||
|
{
|
||||||
|
sof = COMMAND_SOF_DEBUG;
|
||||||
|
payload_len = 37; // 9*4 float + checksum(1)
|
||||||
|
idx = 0;
|
||||||
|
state = 1;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1: // 读取payload
|
||||||
|
fifo_read_element((fifo_struct*)&command_fifo, &buf[idx], FIFO_READ_AND_CLEAN);
|
||||||
|
idx++;
|
||||||
|
if(idx >= payload_len)
|
||||||
|
{
|
||||||
|
// 校验checksum (payload最后一个字节)
|
||||||
|
uint8_t sum = sof;
|
||||||
|
for(uint8_t i = 0; i < payload_len - 1; i++)
|
||||||
|
sum += buf[i];
|
||||||
|
if(buf[payload_len - 1] == sum)
|
||||||
|
{
|
||||||
|
if(sof == COMMAND_SOF_MODE)
|
||||||
|
{
|
||||||
|
// 命令帧: buf[0]=cmd
|
||||||
|
switch(buf[0])
|
||||||
|
{
|
||||||
|
case 0x01: SwitchToAutoMode(); break;
|
||||||
|
case 0x02: SwitchToManualMode(); break;
|
||||||
|
case 0x03: SwitchToDebugMode(); break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(sof == COMMAND_SOF_MANUAL)
|
||||||
|
{
|
||||||
|
command_process_manual(buf);
|
||||||
|
}
|
||||||
|
else if(sof == COMMAND_SOF_DEBUG)
|
||||||
|
{
|
||||||
|
command_process_debug(buf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
state = 0;
|
||||||
}
|
}
|
||||||
state = 0;
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -87,20 +135,137 @@ void command_task()
|
|||||||
//切换到自动模式
|
//切换到自动模式
|
||||||
void SwitchToAutoMode()
|
void SwitchToAutoMode()
|
||||||
{
|
{
|
||||||
|
current_mode = COMMAND_MODE_AUTO;
|
||||||
|
|
||||||
|
/* 清理PID状态 */
|
||||||
|
PIDClear((PIDInstance *)&pid_speed_right);
|
||||||
|
PIDClear((PIDInstance *)&pid_speed_left);
|
||||||
|
PIDClear((PIDInstance *)&pid_yaw);
|
||||||
|
PIDClear((PIDInstance *)&pid_position);
|
||||||
|
|
||||||
|
/* 初始化输入输出值 */
|
||||||
|
//角度参考值设为当前值
|
||||||
|
yaw_ref = imu_angle.yaw;
|
||||||
|
//角度速率值归零
|
||||||
|
angle_velocity = 0;
|
||||||
|
//速度参考值归零
|
||||||
|
motor_speed_right_ref = 0;
|
||||||
|
motor_speed_left_ref = 0;
|
||||||
|
//电机PWM归零
|
||||||
|
motor_set_pwm_right(0);
|
||||||
|
motor_set_pwm_left(0);
|
||||||
|
|
||||||
|
//设定基础直行速度
|
||||||
|
motor_speed_base=AUTO_MODE_BASE_SPEED;
|
||||||
|
|
||||||
|
/* 开启控制任务 */
|
||||||
|
motor_control_task_enable=1;
|
||||||
|
angle_control_task_enable=1;
|
||||||
|
position_control_task_enable=1;
|
||||||
beep_on_time(100);
|
beep_on_time(100);
|
||||||
}
|
}
|
||||||
|
|
||||||
//切换到手动模式
|
//切换到手动模式
|
||||||
void SwitchToManualMode()
|
void SwitchToManualMode()
|
||||||
{
|
{
|
||||||
|
current_mode = COMMAND_MODE_MANUAL;
|
||||||
|
|
||||||
|
/* 立即关闭位置环 */
|
||||||
|
position_control_task_enable=0;
|
||||||
|
|
||||||
|
/* 清理PID状态 */
|
||||||
|
PIDClear((PIDInstance *)&pid_speed_right);
|
||||||
|
PIDClear((PIDInstance *)&pid_speed_left);
|
||||||
|
PIDClear((PIDInstance *)&pid_yaw);
|
||||||
|
|
||||||
|
/* 初始化输入输出值 */
|
||||||
|
//角度参考值设为当前值
|
||||||
|
//yaw_ref = imu_angle.yaw;//暂时关掉，方便在调试模式下更改姿态，然后切回手动模式观察响应
|
||||||
|
//角度速率归零
|
||||||
|
angle_velocity = 0;
|
||||||
|
//速度参考值归零
|
||||||
|
motor_speed_right_ref = 0;
|
||||||
|
motor_speed_left_ref = 0;
|
||||||
|
//电机PWM归零
|
||||||
|
motor_set_pwm_right(0);
|
||||||
|
motor_set_pwm_left(0);
|
||||||
|
|
||||||
|
//基础直行速度归零
|
||||||
|
motor_speed_base=0;
|
||||||
|
|
||||||
|
/* 开启控制任务 */
|
||||||
|
motor_control_task_enable=1;
|
||||||
|
angle_control_task_enable=1;
|
||||||
|
|
||||||
beep_on_time(200);
|
beep_on_time(200);
|
||||||
}
|
}
|
||||||
|
|
||||||
//切换到调试模式
|
//切换到调试模式
|
||||||
void SwitchToDebugMode()
|
void SwitchToDebugMode()
|
||||||
{
|
{
|
||||||
|
current_mode = COMMAND_MODE_DEBUG;
|
||||||
|
|
||||||
|
/* 立即关闭所有控制任务 */
|
||||||
|
motor_control_task_enable=0;
|
||||||
|
angle_control_task_enable=0;
|
||||||
|
position_control_task_enable=0;
|
||||||
|
|
||||||
|
/* 电机PWM设为0 */
|
||||||
|
motor_set_pwm_right(0);
|
||||||
|
motor_set_pwm_left(0);
|
||||||
|
|
||||||
beep_on_time(300);
|
beep_on_time(300);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//解析手动控制帧 0xA6: [motor_speed_base:2 int16_le][angle_velocity:4 float_le]
|
||||||
|
static void command_process_manual(uint8_t *data)
|
||||||
|
{
|
||||||
|
if(current_mode != COMMAND_MODE_MANUAL)
|
||||||
|
return;
|
||||||
|
|
||||||
|
motor_speed_base = (int16_t)(data[0] | (data[1] << 8));
|
||||||
|
union { float f; uint8_t b[4]; } u;
|
||||||
|
u.b[0] = data[2];
|
||||||
|
u.b[1] = data[3];
|
||||||
|
u.b[2] = data[4];
|
||||||
|
u.b[3] = data[5];
|
||||||
|
angle_velocity = u.f;
|
||||||
|
command_send_angle_velocity((float *)&angle_velocity);
|
||||||
|
}
|
||||||
|
|
||||||
|
//解析调试调参帧 0xA8: 9个float (KpKiKd_speed + KpKiKd_yaw + KpKiKd_position)
|
||||||
|
static void command_process_debug(uint8_t *data)
|
||||||
|
{
|
||||||
|
if(current_mode != COMMAND_MODE_DEBUG)
|
||||||
|
return;
|
||||||
|
|
||||||
|
union { float f; uint8_t b[4]; } u[9];
|
||||||
|
for(int i = 0; i < 9; i++)
|
||||||
|
{
|
||||||
|
u[i].b[0] = data[i*4 + 0];
|
||||||
|
u[i].b[1] = data[i*4 + 1];
|
||||||
|
u[i].b[2] = data[i*4 + 2];
|
||||||
|
u[i].b[3] = data[i*4 + 3];
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电机速度环 KpKiKd (左右共用)
|
||||||
|
pid_speed_right.Kp = u[0].f;
|
||||||
|
pid_speed_right.Ki = u[1].f;
|
||||||
|
pid_speed_right.Kd = u[2].f;
|
||||||
|
pid_speed_left.Kp = u[0].f;
|
||||||
|
pid_speed_left.Ki = u[1].f;
|
||||||
|
pid_speed_left.Kd = u[2].f;
|
||||||
|
|
||||||
|
// 角度环 KpKiKd
|
||||||
|
pid_yaw.Kp = u[3].f;
|
||||||
|
pid_yaw.Ki = u[4].f;
|
||||||
|
pid_yaw.Kd = u[5].f;
|
||||||
|
|
||||||
|
// 位置环 KpKiKd
|
||||||
|
pid_position.Kp = u[6].f;
|
||||||
|
pid_position.Ki = u[7].f;
|
||||||
|
pid_position.Kd = u[8].f;
|
||||||
|
}
|
||||||
//DMA发送使用的地址
|
//DMA发送使用的地址
|
||||||
uint8_t angle_velocity_data[6] = {0};
|
uint8_t angle_velocity_data[6] = {0};
|
||||||
//发送速度前馈
|
//发送速度前馈
|
||||||
|
|||||||
+30
-1
@@ -1,14 +1,43 @@
|
|||||||
#ifndef __COMMAND_H
|
#ifndef __COMMAND_H
|
||||||
#define __COMMAND_H
|
#define __COMMAND_H
|
||||||
|
|
||||||
#define COMMAND_FIFO_BUFFER_SIZE 16
|
#define COMMAND_FIFO_BUFFER_SIZE 64
|
||||||
|
|
||||||
#include "zf_common_typedef.h"
|
#include "zf_common_typedef.h"
|
||||||
|
/*
|
||||||
|
---- 手动控制数据帧 0xA6 (10 字节) ----
|
||||||
|
[SOF:0xA6][motor_speed_base:2][angle_velocity:4][checksum:1]
|
||||||
|
motor_speed_base:int16_t 基础直行速度
|
||||||
|
angle_velocity:float 角度速率，单位度每秒
|
||||||
|
手动控制数据帧只有在手动模式下才起作用，其他模式不起作用
|
||||||
|
---- 命令帧 0xA7 (3 字节) ----
|
||||||
|
[SOF:0xA7][cmd:1][checksum:1]
|
||||||
|
cmd: 0x01=自动模式, 0x02=手动模式, 0x03=调试模式
|
||||||
|
---- 调试调参帧 0xA8 (x 字节) ----
|
||||||
|
包括左右电机速度环KpKiKd、角度环KpKiKd、位置环KpKiKd
|
||||||
|
调试调参帧只有在调试模式下才起作用，其他模式不起作用
|
||||||
|
*/
|
||||||
|
#define COMMAND_SOF_MANUAL 0xA6
|
||||||
|
#define COMMAND_SOF_MODE 0xA7
|
||||||
|
#define COMMAND_SOF_DEBUG 0xA8
|
||||||
|
|
||||||
|
#define COMMAND_MANUAL_LEN 8 // SOF(1)+motor_speed_base(2)+angle_velocity(4)+checksum(1)
|
||||||
|
#define COMMAND_MODE_LEN 3 // SOF(1)+cmd(1)+checksum(1)
|
||||||
|
#define COMMAND_DEBUG_LEN 38 // SOF(1)+9*4(KpKiKd)+checksum(1)
|
||||||
|
|
||||||
void command_init();
|
void command_init();
|
||||||
void command_task();
|
void command_task();
|
||||||
void command_send_angle_velocity(float* pointer_angle_velocity);
|
void command_send_angle_velocity(float* pointer_angle_velocity);
|
||||||
|
|
||||||
|
/* 工作模式 */
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
COMMAND_MODE_AUTO = 0, // 自动
|
||||||
|
COMMAND_MODE_MANUAL = 1, // 手动
|
||||||
|
COMMAND_MODE_DEBUG = 2, // 调试
|
||||||
|
} Vision_Mode_e;
|
||||||
|
extern volatile Vision_Mode_e current_mode;
|
||||||
|
|
||||||
void SwitchToAutoMode();
|
void SwitchToAutoMode();
|
||||||
void SwitchToManualMode();
|
void SwitchToManualMode();
|
||||||
void SwitchToDebugMode();
|
void SwitchToDebugMode();
|
||||||
|
|||||||
@@ -143,6 +143,30 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
|
|||||||
pid->dt = config->T;
|
pid->dt = config->T;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 清除PID运行时状态（积分、误差、输出等），保留配置参数不变
|
||||||
|
*
|
||||||
|
* @param pid PID实例指针
|
||||||
|
*/
|
||||||
|
void PIDClear(PIDInstance *pid)
|
||||||
|
{
|
||||||
|
pid->Measure = 0;
|
||||||
|
pid->Last_Measure = 0;
|
||||||
|
pid->Err = 0;
|
||||||
|
pid->Last_Err = 0;
|
||||||
|
pid->Last_ITerm = 0;
|
||||||
|
pid->Pout = 0;
|
||||||
|
pid->Iout = 0;
|
||||||
|
pid->Dout = 0;
|
||||||
|
pid->ITerm = 0;
|
||||||
|
pid->Output = 0;
|
||||||
|
pid->Last_Output = 0;
|
||||||
|
pid->Last_Dout = 0;
|
||||||
|
pid->Ref = 0;
|
||||||
|
pid->ERRORHandler.ERRORCount = 0;
|
||||||
|
pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief PID计算
|
* @brief PID计算
|
||||||
* @param[in] PID结构体
|
* @param[in] PID结构体
|
||||||
|
|||||||
@@ -111,4 +111,11 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
|
|||||||
*/
|
*/
|
||||||
float PIDCalculate(PIDInstance *pid, float measure, float ref);
|
float PIDCalculate(PIDInstance *pid, float measure, float ref);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 清除PID运行时状态（积分、误差、输出等），保留配置参数
|
||||||
|
*
|
||||||
|
* @param pid PID实例指针
|
||||||
|
*/
|
||||||
|
void PIDClear(PIDInstance *pid);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
+7
-6
@@ -7,7 +7,6 @@
|
|||||||
#include "GrayscaleSensor.h"
|
#include "GrayscaleSensor.h"
|
||||||
#include "Motor.h"
|
#include "Motor.h"
|
||||||
#include "MotorEncoder.h"
|
#include "MotorEncoder.h"
|
||||||
#include "PID_Controller.h"
|
|
||||||
#include "Command.h"
|
#include "Command.h"
|
||||||
|
|
||||||
|
|
||||||
@@ -20,9 +19,9 @@ volatile int16_t motor_speed_right_ref = 0;
|
|||||||
volatile int16_t motor_speed_left_ref = 0;
|
volatile int16_t motor_speed_left_ref = 0;
|
||||||
/*角度环接管速度之后的基础直行速度*/
|
/*角度环接管速度之后的基础直行速度*/
|
||||||
volatile int16_t motor_speed_base = 0;
|
volatile int16_t motor_speed_base = 0;
|
||||||
/*角度环输入，开启位置环后由位置环接管*/
|
/*角度环参考值*/
|
||||||
volatile float yaw_ref = 0;
|
volatile float yaw_ref = 0;
|
||||||
/*位置环的输出*/
|
/*角度速率控制，开启位置环后由位置环接管*/
|
||||||
volatile float angle_velocity = 0;
|
volatile float angle_velocity = 0;
|
||||||
|
|
||||||
volatile PIDInstance pid_speed_right={0};
|
volatile PIDInstance pid_speed_right={0};
|
||||||
@@ -61,6 +60,7 @@ void Task_50ms(uint32 state, void *ptr)
|
|||||||
// 50ms 执行一次的任务代码(20Hz)
|
// 50ms 执行一次的任务代码(20Hz)
|
||||||
key_scanner();
|
key_scanner();
|
||||||
beep_actuator();
|
beep_actuator();
|
||||||
|
command_task();
|
||||||
}
|
}
|
||||||
void task_init(void)
|
void task_init(void)
|
||||||
{
|
{
|
||||||
@@ -111,12 +111,15 @@ void angle_control_init(float T)
|
|||||||
.Kd = 0.0f,
|
.Kd = 0.0f,
|
||||||
.T = T,
|
.T = T,
|
||||||
.MaxOut = 25,
|
.MaxOut = 25,
|
||||||
.Improve = PID_Yaw_Error_Corretion
|
.Improve = PID_Yaw_Error_Corretion | PID_Derivative_On_Measurement
|
||||||
};
|
};
|
||||||
PIDInit((PIDInstance *)&pid_yaw, &config_yaw);
|
PIDInit((PIDInstance *)&pid_yaw, &config_yaw);
|
||||||
}
|
}
|
||||||
void angle_control_task(void)
|
void angle_control_task(void)
|
||||||
{
|
{
|
||||||
|
//角度速率控制
|
||||||
|
set_yaw_ref(yaw_ref + angle_velocity*pid_yaw.dt);
|
||||||
|
|
||||||
float speed_dif = PIDCalculate((PIDInstance *)&pid_yaw,imu_angle.yaw,yaw_ref);
|
float speed_dif = PIDCalculate((PIDInstance *)&pid_yaw,imu_angle.yaw,yaw_ref);
|
||||||
motor_speed_left_ref = motor_speed_base - speed_dif;
|
motor_speed_left_ref = motor_speed_base - speed_dif;
|
||||||
motor_speed_right_ref = motor_speed_base + speed_dif;
|
motor_speed_right_ref = motor_speed_base + speed_dif;
|
||||||
@@ -139,8 +142,6 @@ void position_control_task(void)
|
|||||||
|
|
||||||
//发送给云台，角度速率前馈
|
//发送给云台，角度速率前馈
|
||||||
command_send_angle_velocity((float*)&angle_velocity);
|
command_send_angle_velocity((float*)&angle_velocity);
|
||||||
|
|
||||||
set_yaw_ref(yaw_ref + angle_velocity*pid_position.dt);
|
|
||||||
}
|
}
|
||||||
void set_yaw_ref(float set_value)
|
void set_yaw_ref(float set_value)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -3,6 +3,8 @@
|
|||||||
|
|
||||||
#include "zf_common_typedef.h"
|
#include "zf_common_typedef.h"
|
||||||
|
|
||||||
|
#include "PID_Controller.h"
|
||||||
|
|
||||||
void task_init(void);
|
void task_init(void);
|
||||||
|
|
||||||
//建议不直接修改此变量，而是通过set_yaw_ref(float set_value)设置，以防预期之外的值
|
//建议不直接修改此变量，而是通过set_yaw_ref(float set_value)设置，以防预期之外的值
|
||||||
@@ -14,7 +16,20 @@ extern volatile uint8_t motor_control_task_enable;
|
|||||||
extern volatile uint8_t angle_control_task_enable;
|
extern volatile uint8_t angle_control_task_enable;
|
||||||
extern volatile uint8_t position_control_task_enable;
|
extern volatile uint8_t position_control_task_enable;
|
||||||
|
|
||||||
|
extern volatile PIDInstance pid_speed_right;
|
||||||
|
extern volatile PIDInstance pid_speed_left;
|
||||||
|
extern volatile PIDInstance pid_yaw;
|
||||||
|
extern volatile PIDInstance pid_position;
|
||||||
|
|
||||||
|
/*速度环的输入，开启角度环后由角度环接管(范围-50到+50)*/
|
||||||
|
extern volatile int16_t motor_speed_right_ref;
|
||||||
|
extern volatile int16_t motor_speed_left_ref;
|
||||||
|
/*角度环接管速度之后的基础直行速度*/
|
||||||
|
extern volatile int16_t motor_speed_base;
|
||||||
|
/*角度环参考值*/
|
||||||
|
extern volatile float yaw_ref;
|
||||||
|
/*角度速率控制，开启位置环后由位置环接管*/
|
||||||
|
extern volatile float angle_velocity;
|
||||||
//配置项
|
//配置项
|
||||||
/*自动模式下的基础直行速度*/
|
/*自动模式下的基础直行速度*/
|
||||||
#define AUTO_MODE_BASE_SPEED 10
|
#define AUTO_MODE_BASE_SPEED 10
|
||||||
|
|||||||
Reference in New Issue
Block a user