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采样时间配置以优化性能
114 lines
3.4 KiB
C++
114 lines
3.4 KiB
C++
// 函数声明
|
|
#ifndef _FOC_H
|
|
#define _FOC_H
|
|
#include "config.h"
|
|
#include "pid.h"
|
|
#include <cstdint>
|
|
|
|
class FOC_Controller {
|
|
|
|
public:
|
|
FOC_Controller();
|
|
|
|
// 初始化函数
|
|
u8 init();
|
|
|
|
u8 control_mode = 0;
|
|
float limit_voltage = 0.0f;
|
|
|
|
float target_angle = 0.0f;
|
|
float target_velocity = 0.0f;
|
|
float target_current = 0.0f;
|
|
|
|
float rt_angle = 0.0f;
|
|
float rt_velocity = 0.0f;
|
|
float rt_current = 0.0f;
|
|
|
|
bool over_current = false;
|
|
bool voltage_error = false;
|
|
bool is_shut_down = false;
|
|
bool thermal_shutdown = false; // 过热关机标志
|
|
|
|
float voltage_power_supply = 24.0f; // 电源电压
|
|
float v_safe = voltage_power_supply / 1.05f;
|
|
float duty_a = 0, duty_b = 0, duty_c = 0;
|
|
float Ua = 0, Ub = 0, Uc = 0;
|
|
|
|
float cur_loop_p = DEFAULT_current_loop_p;
|
|
float vel_loop_p = DEFAULT_vel_loop_p;
|
|
float ang_loop_p = DEFAULT_angle_loop_p;
|
|
|
|
float vel_angloop_p = DEFAULT_vel_angloop_p;
|
|
float ang_angloop_p = DEFAULT_angle_angloop_p;
|
|
|
|
float ang_MIT_p = DEFAULT_angle_MIT_p;
|
|
float vel_MIT_p = DEFAULT_vel_MIT_p;
|
|
float cur_MIT_p = DEFAULT_current_MIT_p;
|
|
|
|
float velocity_open_loop(void);
|
|
float angle_control(void);
|
|
float angle_control_with_velocity_limit(void);
|
|
float MIT_Mode(void);
|
|
float velocity_control(void);
|
|
float current_control(void);
|
|
|
|
void update_power(float voltage);
|
|
void DFOC_alignSensor(int _PP);
|
|
void shut_down(void);
|
|
void resume(void);
|
|
void drv_stop(void);
|
|
float drv_hold(void);
|
|
void shut_down_thermal(void); // 过热关机
|
|
void resume_from_thermal(void); // 过热恢复
|
|
|
|
void setSVPWM_PhaseVoltage(float Uq, float Ud);
|
|
// 此处默认值不作数
|
|
PIDController vel_loop =
|
|
PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
|
|
vel_loop_Kd, 10000, voltage_power_supply}; // 12V
|
|
PIDController angle_loop =
|
|
PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
|
|
10000000, voltage_power_supply}; // 12V
|
|
PIDController current_loop = PIDController{
|
|
DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
|
|
current_loop_Kd, 1500, voltage_power_supply}; // 12V
|
|
PIDController vel_inVelAngloop =
|
|
PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
|
|
vel_angloop_Kd, 1e6, v_safe};
|
|
PIDController ang_inVelAngloop =
|
|
PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
|
|
ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
|
|
|
|
PIDController ang_inMITloop =
|
|
PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
|
|
angle_MIT_Kd, 1e5, 100};
|
|
PIDController vel_inMITloop = PIDController{
|
|
DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
|
|
PIDController current_inMITloop = PIDController{
|
|
DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
|
|
current_loop_Kd, 1500, voltage_power_supply}; // 12V
|
|
|
|
void update_pid_limits() {
|
|
vel_loop.limit = voltage_power_supply;
|
|
angle_loop.limit = voltage_power_supply;
|
|
current_loop.limit = voltage_power_supply;
|
|
vel_inVelAngloop.limit = voltage_power_supply;
|
|
}
|
|
|
|
protected:
|
|
float shaft_angle = 0;
|
|
float Ualpha, Ubeta = 0;
|
|
float zero_electric_angle = 0;
|
|
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
|
void setPhaseVoltage(float Uq, float Ud);
|
|
float cal_Iq_Id(float current_a, float current_b, float current_c);
|
|
};
|
|
|
|
// 定义函数指针类型
|
|
typedef float (FOC_Controller::*FOCFunctionPtr)(void);
|
|
|
|
void calibrate_zero_position(void);
|
|
inline float _normalizeAngle(float angle);
|
|
inline float _electricalAngle();
|
|
|
|
#endif |