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

实现完整串口指令协议：模式切换、手动控制、PID在线调参

Browse Source 
- 新增 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:
李傲琦
2026-06-07 16:23:10 +08:00
parent d120fa0d17
commit 022e3424da
7 changed files with 488 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
project/code/Command.c
+183 -18
View File
@@ -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};
//发送速度前馈
project/code/Command.h
+30 -1
View File
@@ -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();
project/code/PID_Controller.c
+24
View File
@@ -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结构体
project/code/PID_Controller.h
+7
View File
@@ -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
project/code/Task.c
+7 -6
View File
@@ -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)
{
project/code/Task.h
+15
View File
@@ -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