diff --git a/docs/底盘通信协议指南.md b/docs/底盘通信协议指南.md new file mode 100644 index 0000000..393c7cc --- /dev/null +++ b/docs/底盘通信协议指南.md @@ -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 +#include + +// --- 构造 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 +} +``` diff --git a/project/code/Command.c b/project/code/Command.c index 095b768..e394768 100644 --- a/project/code/Command.c +++ b/project/code/Command.c @@ -5,11 +5,19 @@ #include "ti/driverlib/dl_dma.h" #include "zf_common_fifo.h" #include "Beep.h" +#include "Task.h" +#include "Motor.h" +#include "PID_Controller.h" +#include "IMU.h" volatile fifo_struct command_fifo = {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); +static void command_process_manual(uint8_t *data); +static void command_process_debug(uint8_t *data); + void command_callback(uint32 state, void *ptr) { while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据 @@ -52,33 +60,73 @@ void command_init() void command_task() { 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; while(fifo_used((fifo_struct*)&command_fifo) > 0) { switch(state) { - case 0: // 等待SOF 0xA7 + case 0: // 等待SOF fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN); - if(byte == 0xA7) state = 1; - 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)) + if(byte == COMMAND_SOF_MODE) { - switch(cmd) - { - case 0x01: SwitchToAutoMode(); break; - case 0x02: SwitchToManualMode(); break; - case 0x03: SwitchToDebugMode(); break; - } + sof = COMMAND_SOF_MODE; + payload_len = 2; // cmd(1) + checksum(1) + idx = 0; + state = 1; + } + 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; } } @@ -87,20 +135,137 @@ void command_task() //切换到自动模式 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); } //切换到手动模式 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); } //切换到调试模式 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); } + +//解析手动控制帧 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发送使用的地址 uint8_t angle_velocity_data[6] = {0}; //发送速度前馈 diff --git a/project/code/Command.h b/project/code/Command.h index 6edd242..cbc221f 100644 --- a/project/code/Command.h +++ b/project/code/Command.h @@ -1,14 +1,43 @@ #ifndef __COMMAND_H #define __COMMAND_H -#define COMMAND_FIFO_BUFFER_SIZE 16 +#define COMMAND_FIFO_BUFFER_SIZE 64 #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_task(); 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 SwitchToManualMode(); void SwitchToDebugMode(); diff --git a/project/code/PID_Controller.c b/project/code/PID_Controller.c index 2b2d375..c8df4f7 100644 --- a/project/code/PID_Controller.c +++ b/project/code/PID_Controller.c @@ -143,6 +143,30 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config) 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计算 * @param[in] PID结构体 diff --git a/project/code/PID_Controller.h b/project/code/PID_Controller.h index fbd4259..a68eb8a 100644 --- a/project/code/PID_Controller.h +++ b/project/code/PID_Controller.h @@ -111,4 +111,11 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config); */ float PIDCalculate(PIDInstance *pid, float measure, float ref); +/** + * @brief 清除PID运行时状态（积分、误差、输出等），保留配置参数 + * + * @param pid PID实例指针 + */ +void PIDClear(PIDInstance *pid); + #endif \ No newline at end of file diff --git a/project/code/Task.c b/project/code/Task.c index 1c527d2..d9819b9 100644 --- a/project/code/Task.c +++ b/project/code/Task.c @@ -7,7 +7,6 @@ #include "GrayscaleSensor.h" #include "Motor.h" #include "MotorEncoder.h" -#include "PID_Controller.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_base = 0; -/*角度环输入，开启位置环后由位置环接管*/ +/*角度环参考值*/ volatile float yaw_ref = 0; -/*位置环的输出*/ +/*角度速率控制，开启位置环后由位置环接管*/ volatile float angle_velocity = 0; volatile PIDInstance pid_speed_right={0}; @@ -61,6 +60,7 @@ void Task_50ms(uint32 state, void *ptr) // 50ms 执行一次的任务代码(20Hz) key_scanner(); beep_actuator(); + command_task(); } void task_init(void) { @@ -111,12 +111,15 @@ void angle_control_init(float T) .Kd = 0.0f, .T = T, .MaxOut = 25, - .Improve = PID_Yaw_Error_Corretion + .Improve = PID_Yaw_Error_Corretion | PID_Derivative_On_Measurement }; PIDInit((PIDInstance *)&pid_yaw, &config_yaw); } 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); motor_speed_left_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); - - set_yaw_ref(yaw_ref + angle_velocity*pid_position.dt); } void set_yaw_ref(float set_value) { diff --git a/project/code/Task.h b/project/code/Task.h index 389099f..0b529d3 100644 --- a/project/code/Task.h +++ b/project/code/Task.h @@ -3,6 +3,8 @@ #include "zf_common_typedef.h" +#include "PID_Controller.h" + void task_init(void); //建议不直接修改此变量，而是通过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 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