cbb
c0b5f6902b
- 定义SOFTWARE_VERSION 26.1.19.1 - 定义HARDWARE_VERSION 25.12.01.0 - 定义BOOT_VERSION 26.1.12.0 feat(motor-control): 实现FOC控制算法速度环优化 refactor(can-protocol): 升级CANFD通信协议支持版本管理 - 实现版本号管理和CANFD_VERSION_ID协议 - 扩展CMD_ID消息解析支持PID参数动态配置 - 添加版本管理器实例并实现版本信息发送功能 - 优化CANFD异步发送数据长度判断逻辑 fix(angle-sensor): 修正角度传感器方向和偏移校准 - 添加ANGLESENS_OFFSET补偿参数 - 反转角度传感器读数方向(DIR = -1.0f) - 在角度计算中应用负向因子和偏移量 chore(memory-layout): 调整Flash存储布局和版本配置 - 扩展BOOTLOADER_SIZE从16KB到24KB - 更新APPLATEST_START_ADDR和APPBACK_START_ADDR偏移 - 重新分配OTA配置参数存储区域 - 增加EEP_DEBUG调试开关 refactor(pid-controller): 增强PID控制器功能接口 - 添加set_P_percent方法支持百分比参数设置 - 格式化PID计算代码提升可读性 - 保留积分和微分环节的约束保护机制
70 lines
1.8 KiB
C++
70 lines
1.8 KiB
C++
#include "pid.h"
|
|
#define _constrain(amt, low, high) \
|
|
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
|
|
#include "Can.h"
|
|
#include "math.h"
|
|
|
|
extern CAN_transmiter CAN0_transmiter;
|
|
constexpr float TIME_SLICE = 1e-4;
|
|
|
|
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
|
|
: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
|
|
,
|
|
limit(limit) // PID控制器输出限幅
|
|
,
|
|
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
|
|
|
|
// PID 控制器函数
|
|
float PIDController::operator()(float error) {
|
|
// P环
|
|
float proportional = P * error;
|
|
|
|
// Tustin 散点积分（I环）
|
|
// 添加积分限幅，防止积分饱和
|
|
float integral =
|
|
integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
|
|
integral = _constrain(integral, -limit, limit);
|
|
|
|
// D环（微分环节）
|
|
float derivative = D * (error - error_prev) / TIME_SLICE;
|
|
|
|
// 将P,I,D三环的计算值加起来
|
|
float output = proportional + integral + derivative;
|
|
output = _constrain(output, -limit, limit);
|
|
|
|
// 输出斜坡限制
|
|
// 简化的斜坡限制（单行逻辑）
|
|
if (output_ramp > 0) {
|
|
float max_delta = output_ramp * TIME_SLICE;
|
|
output = output_prev +
|
|
fmaxf(-max_delta, fminf(max_delta, output - output_prev));
|
|
}
|
|
// 保存值（为了下一次循环）
|
|
integral_prev = integral;
|
|
output_prev = output;
|
|
error_prev = error;
|
|
|
|
return output;
|
|
}
|
|
|
|
/**
|
|
* @brief 设置PID参数
|
|
*
|
|
* @param P: P参数
|
|
* @param I: I参数
|
|
* @param D: D参数
|
|
* @return void
|
|
*/
|
|
void PIDController::set_PID(float P, float I, float D) {
|
|
this->P = P;
|
|
this->I = I;
|
|
this->D = D;
|
|
}
|
|
|
|
/**
|
|
* @brief 设置P参数
|
|
*
|
|
* @param P: P参数
|
|
* @return void
|
|
*/
|
|
void PIDController::set_P_percent(float pp) { this->P = pp; } |