cbb
a26dd1cc8e
- 实现循环缓冲区队列管理CAN消息接收 - 添加缓冲区满/空状态检查函数 - 实现消息出队列dequeue_rx_message功能 - 在中断处理程序中添加消息入队列机制 - 优化CAN消息处理流程，支持批量处理 refactor(motor): 优化电机控制类结构和安全机制 - 重命名PID控制器参数变量名以提高可读性 - 添加过热关机标志位thermal_shutdown和相关处理函数 - 实现过热关机shut_down_thermal和过热恢复resume_from_thermal功能 - 调整关机逻辑，添加drv_stop调用确保电机完全停止 - 修正电流环控制中的占空比设置，改为单相保持 feat(motor): 实现FOC控制算法改进和零点校准功能 - 实现电机零点校准函数calibrate_zero_position用于位置补偿 - 修正FOC控制器中的电气角度计算，集成KTH7823校准角度补偿 - 新增d轴电流计算功能 - 重构PID参数初始化方式，使用统一的电压限制配置 - 更新SVPWM相电压设置算法，改进角度归一化处理 - 修改缺省扇区输出占空比从0.0f改为0.5f以提高稳定性 fix(control): 修复控制回路方向和频率计算问题 - 修正角度控制回路中的误差计算方向 - 修正开环速度控制中的频率参数使用 - 修正速度环控制中的误差计算方向 - 修正MIT模式下多级PID控制器的参数设置和误差计算 refactor(filter): 修正编码器速度计算算法 - 移除速度计算中的冗余乘法因子2 feat(pid): 扩展PID控制器功能 - 添加reset方法用于重置PID内部状态 chore(build): 更新开发工具版本和ADC配置 - 更新csolution版本至2.13.0 - 调整ADC采样时间配置以优化性能
91 lines
2.1 KiB
C++
91 lines
2.1 KiB
C++
#include "pid.h"
|
|
#define _constrain(amt, low, high) \
|
|
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
|
|
#include "Can.h"
|
|
#include "config.h"
|
|
#include <algorithm>
|
|
#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;
|
|
}
|