cbb
9955132e0e
- 在velocity_control()函数中添加目标速度突变检测，当速度变化 超过5.0f时自动清零积分项 - 在angle_control_with_velocity_limit()函数中添加相同的速度 突变检测逻辑 - 在MIT_Mode()函数中实现速度环积分清零功能 - 添加reset_integral()方法到PID控制器用于清零积分项 refactor(Filter): 将角度卡尔曼滤波器替换为α-β观测器 - 重构EncoderAngleKalman类，将内部卡尔曼滤波器改为α-β观 测器算法 - 保留原有的接口名称和参数以维持兼容性 - 添加ANGLE_OBSERVER_ALPHA和ANGLE_OBSERVER_BETA配置参数 - 修改速度获取逻辑，直接从观测器状态获取速度估计值 perf(systick): 实现基于DWT的CPU使用率监测 - 使用DWT周期计数器精确测量任务执行周期 - 添加busy_task_enter()和busy_task_exit()函数追踪忙碌周期 - 实现每秒统计CPU占用率并提供get_cpu_usage()接口 - 在CAN数据包中添加CPU使用率传输字段 fix(Can): 修复CANFD数据包中的整数打包问题 - 将16位整数正确拆分为两个字节进行传输 - 修正了test_u16数据的字节序打包方式 chore(config): 更新软件版本号并优化配置参数 - 将版本号从26.4.26更新到26.5.17 - 调整角度/速度α-β观测器的相关增益参数
98 lines
2.2 KiB
C++
98 lines
2.2 KiB
C++
#include "pid.h"
|
|
#define _constrain(amt, low, high) \
|
|
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
|
|
#include "Can.h"
|
|
#include "config.h"
|
|
#include <cmath>
|
|
|
|
extern CAN_transmiter CAN0_transmiter;
|
|
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(float pp) {
|
|
this->P = pp;
|
|
}
|
|
|
|
/**
|
|
* @brief 设置输出限幅
|
|
*
|
|
* @param limit: 输出限幅
|
|
* @return void
|
|
*/
|
|
void PIDController::set_limit(float limit) { this->limit = limit; }
|
|
|
|
/**
|
|
* @brief 重置PID控制器状态
|
|
*
|
|
* @return void
|
|
*/
|
|
void PIDController::reset() {
|
|
error_prev = 0.0f;
|
|
output_prev = 0.0f;
|
|
integral_prev = 0.0f;
|
|
}
|
|
|
|
/**
|
|
* @brief 仅清除PID积分项，保留输出斜坡和误差历史
|
|
*
|
|
* @return void
|
|
*/
|
|
void PIDController::reset_integral() { integral_prev = 0.0f; }
|