refactor(FOC): 移除代码冲突标记并优化FOC控制器实现
- 移除了所有git冲突标记（<<<<<<< HEAD, =======, >>>>>>>） - 清理了校准步骤相关注释代码 - 修复了Iq,Id计算中的角度处理逻辑 - 优化了PID控制器参数设置和初始化流程 - 重构了SVPWM相电压设置方法，移除了重复的相电压设置函数 - 修复了关闭功能和电源电压安全限制的计算 feat(CAN): 增强CAN通信协议支持多种控制模式 - 实现了目标电流、速度、角度的多参数控制 - 添加了PID参数动态配置功能 - 支持7种不同控制模式的切换 - 实现了控制模式切换时PID状态重置机制 refactor(pid): 扩展PID控制器功能接口 - 添加了set_P方法替代原有的set_P_percent - 新增set_limit方法用于输出限幅控制 - 实现了reset方法用于PID状态重置 - 优化了PID参数设置接口 fix(timer): 优化定时器任务调度机制 - 重构了任务调度的时间片管理逻辑 - 实现了基于查找表的任务分配方式 - 优化了100us、1ms、10ms、100ms等不同周期的任务调度 - 提高了系统实时性表现 perf(config): 优化系统配置和数学常量定义 - 添加了必要的GD32头文件包含 - 重新组织了PID参数命名规范 - 定义了常用数学常量如三角函数相关数值 - 优化了定时器频率和周期计算公式
This commit is contained in:
+38
-272
@@ -25,7 +25,6 @@ extern CurrentKalman Current_kalman;
|
||||
#define constrain(value, min, max, target_min, target_max) \
|
||||
((value - min) / (max - min) * (target_max - target_min))
|
||||
|
||||
<<<<<<< HEAD
|
||||
// 校准步骤（代码模板）
|
||||
void calibrate_zero_position(void) {
|
||||
// Set_3Pwm(.25, 0, 0); // 起劲
|
||||
@@ -50,23 +49,14 @@ void calibrate_zero_position(void) {
|
||||
// Set_3Pwm(0, 0, 0); // 停止
|
||||
}
|
||||
|
||||
=======
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
|
||||
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
float current_c) {
|
||||
// 计算电气角度
|
||||
<<<<<<< HEAD
|
||||
float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
|
||||
=======
|
||||
float angle_el = fmod(shaft_angle * PP, 360.0f);
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
|
||||
// Clarke变换 (使用三相电流)
|
||||
<<<<<<< HEAD
|
||||
float I_alpha, I_beta;
|
||||
=======
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
|
||||
(1.0f / 3.0f) * current_c;
|
||||
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
|
||||
@@ -76,17 +66,11 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
float I_q, I_d;
|
||||
I_q = I_beta * cos_el - I_alpha * sin_el;
|
||||
<<<<<<< HEAD
|
||||
I_d = I_beta * sin_el + I_alpha * cos_el;
|
||||
|
||||
// 返回Iq值，乘以1000以转换为mA
|
||||
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
|
||||
// return I_q * 1000 - KE_FACTOR * rt_velocity;
|
||||
=======
|
||||
|
||||
return I_q * 1000.0f -
|
||||
KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -96,7 +80,6 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
*/
|
||||
u8 FOC_Controller::init() {
|
||||
motor_init(); // 电机驱动初始化
|
||||
<<<<<<< HEAD
|
||||
|
||||
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
|
||||
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
|
||||
@@ -106,22 +89,6 @@ u8 FOC_Controller::init() {
|
||||
FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
|
||||
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
|
||||
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
|
||||
=======
|
||||
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
|
||||
vel_loop_Kd); // 设置电机速度控制参数
|
||||
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
|
||||
0.2f); // 设置电机角度控制参数
|
||||
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
|
||||
7e-8); // 设置电机电流控制参数
|
||||
|
||||
vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
|
||||
vel_angloop_Kd); // 设置电机速度控制参数
|
||||
ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
|
||||
ang_angloop_Ki,
|
||||
ang_angloop_Kd); // 设置电机角度控制参数
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
|
||||
CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -131,150 +98,49 @@ u8 FOC_Controller::init() {
|
||||
*/
|
||||
FOC_Controller::FOC_Controller() { ; }
|
||||
|
||||
/**
|
||||
* @brief 设置电机输出
|
||||
*
|
||||
* @param Uq: q轴电压
|
||||
* @param Ud: d轴电压
|
||||
* @param angle_el: 电机轴角度
|
||||
*/
|
||||
inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
|
||||
// 计算电气角度
|
||||
float angle_el = fmod(shaft_angle * PP, 360.0f);
|
||||
// /**
|
||||
// * @brief 设置电机输出
|
||||
// *
|
||||
// * @param Uq: q轴电压
|
||||
// * @param Ud: d轴电压
|
||||
// * @param angle_el: 电机轴角度
|
||||
// */
|
||||
// inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
|
||||
// // 计算电气角度
|
||||
// float angle_el = fmod(shaft_angle * PP, 360.0f);
|
||||
|
||||
// 帕克逆变换 (直接计算，避免函数调用开销)
|
||||
float sin_el, cos_el;
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
float Ualpha = Ud * cos_el - Uq * sin_el;
|
||||
float Ubeta = Ud * sin_el + Uq * cos_el;
|
||||
// // 帕克逆变换 (直接计算，避免函数调用开销)
|
||||
// float sin_el, cos_el;
|
||||
// arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
// float Ualpha = Ud * cos_el - Uq * sin_el;
|
||||
// float Ubeta = Ud * sin_el + Uq * cos_el;
|
||||
|
||||
// 克拉克逆变换 (使用预计算常量优化)
|
||||
Ua = Ualpha + v_half;
|
||||
Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
|
||||
Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;
|
||||
// // 克拉克逆变换 (使用预计算常量优化)
|
||||
// Ua = Ualpha + v_half;
|
||||
// Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
|
||||
// Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;
|
||||
|
||||
if (Ua > v_safe)
|
||||
Ua = v_safe;
|
||||
else if (Ua < -v_safe)
|
||||
Ua = -v_safe;
|
||||
if (Ub > v_safe)
|
||||
Ub = v_safe;
|
||||
else if (Ub < -v_safe)
|
||||
Ub = -v_safe;
|
||||
if (Uc > v_safe)
|
||||
Uc = v_safe;
|
||||
else if (Uc < -v_safe)
|
||||
Uc = -v_safe;
|
||||
// if (Ua > v_safe)
|
||||
// Ua = v_safe;
|
||||
// else if (Ua < -v_safe)
|
||||
// Ua = -v_safe;
|
||||
// if (Ub > v_safe)
|
||||
// Ub = v_safe;
|
||||
// else if (Ub < -v_safe)
|
||||
// Ub = -v_safe;
|
||||
// if (Uc > v_safe)
|
||||
// Uc = v_safe;
|
||||
// else if (Uc < -v_safe)
|
||||
// Uc = -v_safe;
|
||||
|
||||
// 设置PWM输出
|
||||
float voltage_exchange = limit_voltage / v_safe;
|
||||
duty_a = Ua / voltage_power_supply * voltage_exchange;
|
||||
duty_b = Ub / voltage_power_supply * voltage_exchange;
|
||||
duty_c = Uc / voltage_power_supply * voltage_exchange;
|
||||
// // 设置PWM输出
|
||||
// float voltage_exchange = limit_voltage / v_safe;
|
||||
// duty_a = Ua / voltage_power_supply * voltage_exchange;
|
||||
// duty_b = Ub / voltage_power_supply * voltage_exchange;
|
||||
// duty_c = Uc / voltage_power_supply * voltage_exchange;
|
||||
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
Set_3Pwm(duty_a, duty_b, duty_c);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用SVPWM算法设置电机输出（优化版）
|
||||
*
|
||||
* @param Uq: q轴电压
|
||||
* @param Ud: d轴电压
|
||||
*/
|
||||
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
|
||||
// 计算电气角度（保持原有角度单位和范围）
|
||||
float angle_el = fmod(shaft_angle * PP, 360.0f);
|
||||
|
||||
// 帕克逆变换
|
||||
float sin_el, cos_el;
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
float Ualpha = Ud * cos_el - Uq * sin_el;
|
||||
float Ubeta = Ud * sin_el + Uq * cos_el;
|
||||
|
||||
// 计算角度并归一化到 [0, 2π)
|
||||
float angle = atan2f(Ubeta, Ualpha);
|
||||
if (angle < 0)
|
||||
angle += 2.0f * PI;
|
||||
|
||||
// 扇区与局部角度
|
||||
int sector = (int)(angle * INV_PI_3) + 1;
|
||||
if (sector < 1)
|
||||
sector = 1;
|
||||
else if (sector > 6)
|
||||
sector = 6;
|
||||
float local_angle = angle - (sector - 1) * PI_3;
|
||||
|
||||
// 模长计算
|
||||
float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);
|
||||
|
||||
// 计算电压调制比，同时限制最大输出电压
|
||||
float modulation_index =
|
||||
(magnitude / voltage_power_supply); // 90%作为安全裕量
|
||||
|
||||
// 电压限制
|
||||
if (limit_voltage > voltage_power_supply) {
|
||||
limit_voltage = voltage_power_supply;
|
||||
}
|
||||
float limit_percent = limit_voltage / v_safe;
|
||||
if (modulation_index > limit_percent) {
|
||||
// 限制电压
|
||||
modulation_index = limit_percent;
|
||||
// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
|
||||
magnitude = voltage_power_supply * limit_percent;
|
||||
}
|
||||
|
||||
// 使用arm_sin_cos_f32计算local_angle的sin和cos
|
||||
float sin_l, cos_l;
|
||||
arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
|
||||
float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
|
||||
float T2 = modulation_index * sin_l;
|
||||
float T0 = 1.0f - T1 - T2;
|
||||
|
||||
// 计算占空比（按规范化时间映射）
|
||||
switch (sector) {
|
||||
case 1:
|
||||
duty_a = 0.5f * T0 + T1 + T2;
|
||||
duty_b = 0.5f * T0 + T2;
|
||||
duty_c = 0.5f * T0;
|
||||
break;
|
||||
case 2:
|
||||
duty_a = 0.5f * T0 + T1;
|
||||
duty_b = 0.5f * T0 + T1 + T2;
|
||||
duty_c = 0.5f * T0;
|
||||
break;
|
||||
case 3:
|
||||
duty_a = 0.5f * T0;
|
||||
duty_b = 0.5f * T0 + T1 + T2;
|
||||
duty_c = 0.5f * T0 + T2;
|
||||
break;
|
||||
case 4:
|
||||
duty_a = 0.5f * T0;
|
||||
duty_b = 0.5f * T0 + T1;
|
||||
duty_c = 0.5f * T0 + T1 + T2;
|
||||
break;
|
||||
case 5:
|
||||
duty_a = 0.5f * T0 + T2;
|
||||
duty_b = 0.5f * T0;
|
||||
duty_c = 0.5f * T0 + T1 + T2;
|
||||
break;
|
||||
case 6:
|
||||
duty_a = 0.5f * T0 + T1 + T2;
|
||||
duty_b = 0.5f * T0;
|
||||
duty_c = 0.5f * T0 + T1;
|
||||
break;
|
||||
default:
|
||||
duty_a = 0.0f;
|
||||
duty_b = 0.0f;
|
||||
duty_c = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
// 设置PWM输出
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
Set_3Pwm(duty_a, duty_b, duty_c);
|
||||
}
|
||||
// Set_3Pwm(duty_a, duty_b, duty_c);
|
||||
// }
|
||||
|
||||
/**
|
||||
* @brief 使用SVPWM算法设置电机输出
|
||||
@@ -382,11 +248,7 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
|
||||
*
|
||||
*/
|
||||
void FOC_Controller::shut_down() {
|
||||
<<<<<<< HEAD
|
||||
drv_stop();
|
||||
=======
|
||||
is_shut_down = 1;
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
motor_sleep();
|
||||
is_shut_down = true;
|
||||
}
|
||||
@@ -450,11 +312,7 @@ void FOC_Controller::update_power(float voltage) {
|
||||
voltage_error = false;
|
||||
voltage_power_supply = voltage;
|
||||
v_half = voltage * 0.5f;
|
||||
<<<<<<< HEAD
|
||||
v_safe = voltage * 1.1f;
|
||||
=======
|
||||
v_safe = voltage * 1.05f;
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
} else {
|
||||
voltage_error = true;
|
||||
}
|
||||
@@ -469,7 +327,6 @@ void FOC_Controller::update_power(float voltage) {
|
||||
float FOC_Controller::velocity_open_loop() {
|
||||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
|
||||
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
|
||||
<<<<<<< HEAD
|
||||
MOTOR_FREQUENCY / 2,
|
||||
360.0f);
|
||||
// 获取电机轴角度
|
||||
@@ -481,19 +338,6 @@ float FOC_Controller::velocity_open_loop() {
|
||||
// 因为在开环控制中，Ud始终为0，只控制Uq
|
||||
const float Uq = voltage_power_supply / 2.0f;
|
||||
setSVPWM_PhaseVoltage(Uq, 0.0f);
|
||||
=======
|
||||
TIMER2_FREQUENCY,
|
||||
360.0f);
|
||||
|
||||
// 因为在开环控制中，Ud始终为0，只控制Uq
|
||||
const float Uq = voltage_power_supply / 2.0f;
|
||||
|
||||
setSVPWM_PhaseVoltage(Uq, 0.0f);
|
||||
// CAN0_transmiter.testfloat_union.f = Uq;
|
||||
// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
// sampler.get_current('W'));
|
||||
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
return limit_voltage;
|
||||
}
|
||||
|
||||
@@ -513,84 +357,6 @@ float FOC_Controller::angle_control() {
|
||||
sampler.get_current('W'));
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq, 0.0f);
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
return shaft_angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 带速度限制的角度闭环控制
|
||||
*
|
||||
* @param target_angle: 目标角度
|
||||
* @param max_velocity: 最大速度限制 (单位: rad/s)
|
||||
* @return float: 当前角度
|
||||
*/
|
||||
float FOC_Controller::angle_control_with_velocity_limit() {
|
||||
// 计算角度误差
|
||||
float angle_error = target_angle - rt_angle;
|
||||
|
||||
// 使用角度PID控制器计算目标速度
|
||||
float target_velocity = ang_inVelAngloop(angle_error);
|
||||
|
||||
// 使用速度PID控制器计算最终输出电压
|
||||
// 根据目标速度大小，选择不同的PID参数
|
||||
if (target_velocity < -120.0f) {
|
||||
vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
|
||||
} else if (target_velocity < -30.0f) {
|
||||
vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
|
||||
} else {
|
||||
vel_inVelAngloop.set_P(vel_angloop_p);
|
||||
}
|
||||
float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
|
||||
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'));
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq, 0.0f);
|
||||
|
||||
return shaft_angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 带速度限制的角度闭环控制
|
||||
*
|
||||
* @param target_angle: 目标角度
|
||||
* @param max_velocity: 最大速度限制 (单位: rad/s)
|
||||
* @return float: 当前角度
|
||||
*/
|
||||
float FOC_Controller::MIT_Mode() {
|
||||
// 第一层：角度环（外环）
|
||||
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
|
||||
float angle_error = rt_angle - target_angle;
|
||||
float target_velocity =
|
||||
ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
|
||||
|
||||
// 第二层：速度环（中环）
|
||||
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
|
||||
float velocity_error = target_velocity - rt_velocity;
|
||||
float target_current =
|
||||
vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
|
||||
|
||||
CAN0_transmiter.testfloat_union.f = target_current;
|
||||
// 第三层：电流环（内环）
|
||||
// 计算当前实际电流
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'));
|
||||
|
||||
// 通过电流环PID控制器计算输出电压
|
||||
float current_error = target_current - rt_current;
|
||||
float Uq = current_loop(current_error);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq, 0.0f);
|
||||
|
||||
>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
|
||||
return shaft_angle;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user