cbb
7c4aaa5ee6
- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值 以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的 震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待， 提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益 无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定 性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络， 用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
86 lines
2.4 KiB
C++
86 lines
2.4 KiB
C++
/**
|
|
* @file Algorithm/pid.h
|
|
* @brief 算法模块源文件
|
|
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
|
*/
|
|
|
|
#ifndef PID_H
|
|
#define PID_H
|
|
|
|
/**
|
|
* @brief PIDController 类或结构体
|
|
*/
|
|
class PIDController {
|
|
public:
|
|
PIDController(float P, float I, float D, float ramp, float limit);
|
|
~PIDController() = default;
|
|
|
|
/**
|
|
* @brief PID 计算（唯一入口）
|
|
* @param[in] error 控制误差
|
|
* @param[in] measurement 反馈测量量（D 项对它微分，目标阶跃不产生微分踢）
|
|
* @return float PID 输出
|
|
*/
|
|
float operator()(float error, float measurement);
|
|
/**
|
|
* @brief 设置 set_PID 对应的参数
|
|
* @param[in] P 参数
|
|
* @param[in] I 参数
|
|
* @param[in] D 参数
|
|
* @return void
|
|
*/
|
|
void set_PID(float P, float I, float D);
|
|
/**
|
|
* @brief 设置 set_P 对应的参数
|
|
* @param[in] pp 参数
|
|
* @return void
|
|
*/
|
|
void set_P(float pp);
|
|
/**
|
|
* @brief 设置 set_limit 对应的参数
|
|
* @param[in] limit 参数
|
|
* @return void
|
|
*/
|
|
void set_limit(float limit);
|
|
/**
|
|
* @brief 复位对象内部状态
|
|
* @return void
|
|
*/
|
|
void reset(); // 重置PID控制器状态
|
|
/**
|
|
* @brief reset_integral 函数
|
|
* @return void
|
|
*/
|
|
void reset_integral(); // 仅清除积分项，保留输出斜坡状态
|
|
/**
|
|
* @brief 外部饱和泄放积分（反算抗饱和的补充入口）
|
|
* @param[in] excess 输出超出限幅的部分（带符号），如前馈叠加后超限
|
|
* @return void
|
|
* @note 调用方向须与 PID 输出到最终 Uq 的符号一致（有 ×DIR 的要乘上 DIR）
|
|
*/
|
|
void bleed_integral(float excess);
|
|
|
|
float P; //!< 比例增益(P环增益)
|
|
float I; //!< 积分增益（I环增益）
|
|
float D; //!< 微分增益（D环增益）
|
|
float output_ramp;
|
|
float limit;
|
|
|
|
protected:
|
|
/**
|
|
* @brief 公共计算路径：积分、抗饱和、输出斜坡
|
|
* @param[in] error 控制误差
|
|
* @param[in] derivative 已完成低通滤波的微分项（D 已乘入）
|
|
* @return float PID 输出
|
|
*/
|
|
float step(float error, float derivative);
|
|
|
|
float error_prev; //!< 最后的跟踪误差值
|
|
float output_prev; //!< 最后一个 pid 输出值
|
|
float integral_prev; //!< 最后一个积分分量值
|
|
float measurement_prev; //!< 上次测量值（对测量微分用）
|
|
float derivative_filt; //!< 微分项一阶低通滤波后的变化率
|
|
bool deriv_init; //!< 测量微分是否已完成首次采样
|
|
};
|
|
|
|
#endif |