cbb
971374d8ec
- 重构setPhaseVoltage函数实现，提高执行效率 - 增加update_power函数，动态更新电源电压 - 优化velocity_open_loop函数，调用、setPhaseVoltage避免重复代码 - 实现angle_control和velocity_control闭环控制函数 - 调整PID控制器参数初始化及增加参数设置接口 - 改进电流和编码器速度卡尔曼滤波器实现 - 更新CAN通信数据打包逻辑，增加测试字段 - 在定时器中断中集成多种控制模式切换逻辑 - 完善角度传感器数据处理，改进速度计算方法 - 减小速度滤波数组大小，提升响应速度
66 lines
1.8 KiB
C++
66 lines
1.8 KiB
C++
#include "pid.h"
|
|
#include "Timer.h"
|
|
#define _constrain(amt, low, high) \
|
|
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
|
|
#include "Can.h"
|
|
|
|
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) {
|
|
timestamp_prev = get_system_runtime_us();
|
|
}
|
|
|
|
// PID 控制器函数
|
|
float PIDController::operator()(float error) {
|
|
// 计算两次循环中间的间隔时间
|
|
unsigned long timestamp_now = get_system_runtime_us();
|
|
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
|
|
if (Ts <= 0 || Ts > 0.5f)
|
|
Ts = 1e-3f;
|
|
|
|
// P环
|
|
float proportional = P * error;
|
|
// Tustin 散点积分（I环）
|
|
float integral = integral_prev + I * Ts * 0.5f * (error + error_prev);
|
|
integral = _constrain(integral, -limit, limit);
|
|
// D环（微分环节）
|
|
float derivative = D * (error - error_prev) / Ts;
|
|
// 将P,I,D三环的计算值加起来
|
|
float output = proportional + integral + derivative;
|
|
output = _constrain(output, -limit, limit);
|
|
|
|
if (output_ramp > 0) {
|
|
// 对PID的变化速率进行限制
|
|
float output_rate = (output - output_prev) / Ts;
|
|
if (output_rate > output_ramp)
|
|
output = output_prev + output_ramp * Ts;
|
|
else if (output_rate < -output_ramp)
|
|
output = output_prev - output_ramp * Ts;
|
|
}
|
|
// 保存值（为了下一次循环）
|
|
integral_prev = integral;
|
|
output_prev = output;
|
|
error_prev = error;
|
|
timestamp_prev = timestamp_now;
|
|
|
|
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) {
|
|
P = P;
|
|
I = I;
|
|
D = D;
|
|
} |