SkyLiCoder
022e3424da
- 新增 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
121 lines
3.1 KiB
C
121 lines
3.1 KiB
C
#ifndef _PID_CONTROLLER_H
|
|
#define _PID_CONTROLLER_H
|
|
|
|
#include "zf_common_typedef.h"
|
|
|
|
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
|
|
typedef enum
|
|
{
|
|
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
|
|
PID_Integral_Limit = 0b00000001, // 0000 0001
|
|
PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
|
|
PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
|
|
PID_Yaw_Error_Corretion = 0b00001000, // 0000 1000
|
|
PID_OutputFilter = 0b00010000, // 0001 0000
|
|
PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
|
|
PID_DerivativeFilter = 0b01000000, // 0100 0000
|
|
PID_ErrorHandle = 0b10000000, // 1000 0000
|
|
} PID_Improvement_e;
|
|
|
|
/* PID 报错类型枚举*/
|
|
typedef enum errorType_e
|
|
{
|
|
PID_ERROR_NONE = 0x00U,
|
|
PID_MOTOR_BLOCKED_ERROR = 0x01U
|
|
} ErrorType_e;
|
|
|
|
typedef struct
|
|
{
|
|
uint64_t ERRORCount;
|
|
ErrorType_e ERRORType;
|
|
} PID_ErrorHandler_t;
|
|
|
|
/* PID结构体 */
|
|
typedef struct
|
|
{
|
|
//---------------------------------- init config block
|
|
// config parameter
|
|
float Kp;
|
|
float Ki;
|
|
float Kd;
|
|
float MaxOut;
|
|
float DeadBand;
|
|
|
|
// improve parameter
|
|
PID_Improvement_e Improve;
|
|
float IntegralLimit; // 积分限幅
|
|
float CoefA; // 变速积分 For Changing Integral
|
|
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
|
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
|
|
float Derivative_LPF_RC; // 微分滤波器系数
|
|
|
|
//-----------------------------------
|
|
// for calculating
|
|
float Measure;
|
|
float Last_Measure;
|
|
float Err;
|
|
float Last_Err;
|
|
float Last_ITerm;
|
|
|
|
float Pout;
|
|
float Iout;
|
|
float Dout;
|
|
float ITerm;
|
|
|
|
float Output;
|
|
float Last_Output;
|
|
float Last_Dout;
|
|
|
|
float Ref;
|
|
|
|
float dt;
|
|
|
|
PID_ErrorHandler_t ERRORHandler;
|
|
} PIDInstance;
|
|
|
|
/* 用于PID初始化的结构体*/
|
|
typedef struct // config parameter
|
|
{
|
|
// basic parameter
|
|
float Kp;
|
|
float Ki;
|
|
float Kd;
|
|
float MaxOut; // 输出限幅
|
|
float DeadBand; // 死区
|
|
float T;
|
|
|
|
// improve parameter
|
|
PID_Improvement_e Improve;
|
|
float IntegralLimit; // 积分限幅
|
|
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
|
|
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
|
float Output_LPF_RC; // RC = 1/omegac
|
|
float Derivative_LPF_RC;
|
|
} PID_Init_Config_s;
|
|
|
|
/**
|
|
* @brief 初始化PID实例
|
|
* @todo 待修改为统一的PIDRegister风格
|
|
* @param pid PID实例指针
|
|
* @param config PID初始化配置
|
|
*/
|
|
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
|
|
|
|
/**
|
|
* @brief 计算PID输出
|
|
*
|
|
* @param pid PID实例指针
|
|
* @param measure 反馈值
|
|
* @param ref 设定值
|
|
* @return float PID计算输出
|
|
*/
|
|
float PIDCalculate(PIDInstance *pid, float measure, float ref);
|
|
|
|
/**
|
|
* @brief 清除PID运行时状态（积分、误差、输出等），保留配置参数
|
|
*
|
|
* @param pid PID实例指针
|
|
*/
|
|
void PIDClear(PIDInstance *pid);
|
|
|
|
#endif |