From db2d08032c5c4b4eb08cf66e8435d726371d18ad Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 12 Nov 2025 00:46:26 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E4=BC=98=E5=8C=96FOC=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E9=80=BB=E8=BE=91=E4=B8=8E=E7=83=AD=E7=AE=A1=E7=90=86?= =?UTF-8?q?=E6=9C=BA=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 移除冗余的三值约束宏定义 `_constrain3` - 修改关机逻辑，引入 `thermal_uncontrol` 标志替代旧 shut_down 计数方式 - 调整恢复函数中错误标志位清零逻辑 - 将部分 uint8_t 类型成员变量改为 bool 类型以提高可读性 - 在类中添加电源电压成员变量并调整 PID 参数初始化 - 移除 PID 控制器中不必要的 CAN 测试代码 - 添加 thermal 控制相关文件引用及初始化 - 使用 switch 替代多重 if-else 提高控制模式处理效率 - 引入基于定时器的任务负载均衡调度机制 - 初始化热量控制器并在主循环中调用更新逻辑 --- Robot_V2/Algorithm/DengFOC.cpp | 11 +--- Robot_V2/Algorithm/DengFOC.h | 13 ++-- Robot_V2/Algorithm/pid.cpp | 2 - Robot_V2/Algorithm/thermal.cpp | 42 +++++++++++++ Robot_V2/Algorithm/thermal.h | 27 ++++++++ Robot_V2/Robot_V2.cproject.yml | 2 + Robot_V2/device/Can.cpp | 2 - Robot_V2/device/Timer.cpp | 109 +++++++++++++++++++-------------- Robot_V2/main.cpp | 2 + 9 files changed, 145 insertions(+), 65 deletions(-) create mode 100644 Robot_V2/Algorithm/thermal.cpp create mode 100644 Robot_V2/Algorithm/thermal.h diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 77a7e86..2993eaa 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -33,11 +33,6 @@ CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); // 初始变量及函数定义 #define _constrain(amt, low, high) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) - -// 三值约束比较函数，用于SVPWM算法 -#define _constrain3(a, b, c) \ - ((a) < (b) ? ((a) < (c) ? (a) : (c)) : ((b) < (c) ? (b) : (c))) - #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) @@ -97,7 +92,8 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { * */ void FOC_Controlroller::shut_down() { - is_shut_down = 100; + thermal_uncontrol = 0; + is_shut_down = 1; gpio_bit_reset(GPIOB, GPIO_PIN_12); } /** @@ -105,8 +101,7 @@ void FOC_Controlroller::shut_down() { * */ void FOC_Controlroller::resume() { - over_current = 0; - temp_error = 0; + thermal_uncontrol = 0; voltage_error = 0; is_shut_down = 0; gpio_bit_set(GPIOB, GPIO_PIN_12); diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index da9b995..ed04b15 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -7,17 +7,17 @@ class FOC_Controlroller { public: FOC_Controlroller(); - float control_mode = 0; + u8 control_mode = 0; float limit_voltage = 0.0f; float limit_velocity = 0.0f; float target_angle = 0.0f; float target_velocity = 0.0f; float rt_angle = 0.0f; float rt_velocity = 0.0f; - u8 over_current = 0; - u8 temp_error = 0; - u8 voltage_error = 0; - u8 is_shut_down = 0; + bool thermal_uncontrol = 0; + bool voltage_error = 0; + bool is_shut_down = 0; + float voltage_power_supply = 24.0f; // 电源电压 float velocity_open_loop(); float angle_control(float target_angle); @@ -35,10 +35,9 @@ class FOC_Controlroller { float shaft_angle = 0; float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0; float zero_electric_angle = 0; - float voltage_power_supply = 24.0f; // 电源电压 float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - PIDController vel_loop =PIDController{1.75f, 1.0f, 0.0002f, 1000000, voltage_power_supply}; + PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000, voltage_power_supply}; PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000, voltage_power_supply}; diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 0f3af8c..16a2006 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -29,7 +29,6 @@ float PIDController::operator()(float error) { // 将P,I,D三环的计算值加起来 float output = proportional + integral + derivative; - CAN0_transmiter.testfloat_union.f = D; output = _constrain(output, -limit, limit); // 输出斜坡限制 @@ -41,7 +40,6 @@ float PIDController::operator()(float error) { else if (output_rate < -output_ramp) output = output_prev - output_ramp * TIME_SLICE; } - CAN0_transmiter.testfloat_union.f = output; // 保存值（为了下一次循环） integral_prev = integral; output_prev = output; diff --git a/Robot_V2/Algorithm/thermal.cpp b/Robot_V2/Algorithm/thermal.cpp new file mode 100644 index 0000000..a25faff --- /dev/null +++ b/Robot_V2/Algorithm/thermal.cpp @@ -0,0 +1,42 @@ +#include "thermal.h" +#include "Can.h" +#include "DengFOC.h" +#include "adc.h" +#include + +extern sampler sampler; +extern CAN_transmiter CAN0_transmiter; +extern FOC_Controlroller FOC; +#define SQUARE(x) ((x) * (x)) +#define max(a, b, c) \ + ((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c))) + +constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms +u16 ThermalController::update() { + thermal_value = thermal_value * thermalRate; + thermal_value += + SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp()); + + if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热 + if (thermal_value < thermal_capacity * recoveryRate) { + remainingTime10Ms = 1; // 可以恢复工作 + } + if (remainingTime10Ms == 1) { // 过热恢复 + FOC.resume(); + return --remainingTime10Ms; + } + return --remainingTime10Ms; + } else { // 当前没有过热停机 + current_max = + 1000 * max(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); // 获取电流最大值 + thermal_value += + SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值 + } + if (thermal_value > thermal_capacity) { + FOC.shut_down(); + remainingTime10Ms = PROTECT_TIME; // 过热停机10秒 + return remainingTime10Ms; + } + return remainingTime10Ms; +} \ No newline at end of file diff --git a/Robot_V2/Algorithm/thermal.h b/Robot_V2/Algorithm/thermal.h new file mode 100644 index 0000000..ab17786 --- /dev/null +++ b/Robot_V2/Algorithm/thermal.h @@ -0,0 +1,27 @@ +#ifndef THERMAL_H +#define THERMAL_H +#include "stdint.h" + + +class ThermalController { + private: + u8 temperature; + u16 current_max; + + float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例） + float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作） + + u32 thermal_value = 0; + u32 thermal_capacity = 2250000; // 热容量 + + u16 gapTimeMs = 10; // 单次计算间隔ms + u16 remainingTime10Ms = 0; // 过热冷却剩余ms + public: + u16 update(); + void reset() { + thermal_value = 0; + remainingTime10Ms = 0; + } +}; + +#endif \ No newline at end of file diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index 6fff240..c3ff611 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -29,6 +29,8 @@ project: - file: Algorithm/DengFOC.h - file: Algorithm/pid.cpp - file: pid.h + - file: Algorithm/thermal.cpp + - file: Algorithm/thermal.h components: - component: ARM::CMSIS:CORE - component: Device:Startup diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 07bed66..16642c1 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -668,13 +668,11 @@ void CAN_transmiter::communication_check() { FOC.limit_voltage = float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; - FOC.is_shut_down = 100; // 1s滤除启动电流 } if (unpack_bytes_to_float(receive_message.rx_data, 3).f != FOC.limit_velocity) { FOC.limit_velocity = unpack_bytes_to_float(receive_message.rx_data, 3).f; - FOC.is_shut_down = 100; // 1s滤除启动电流 } target_angle_union.f = unpack_bytes_to_float(receive_message.rx_data, 7).f; diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 2d3651f..04e02d9 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -10,6 +10,7 @@ #include "gd32c10x_misc.h" #include "gd32c10x_rcu.h" #include "gd32c10x_timer.h" +#include "thermal.h" #include #include #include @@ -38,6 +39,7 @@ extern FOC_Controlroller FOC; extern sampler sampler; extern angle_sensor KTH7823; extern EncoderVelocityKalman Vel_kalman; +extern ThermalController thermalController; // 热量控制器初始化 /** * @brief 设置引脚中断优先级 @@ -174,60 +176,81 @@ void TIMER2_IRQHandler(void) { KTH7823.update(); // 更新角度传感器值 FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity()); FOC.rt_angle = KTH7823.getAngle(); - - if (FOC.control_mode == 1) { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 - } else if (FOC.control_mode == 2) { - FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 - } else if (FOC.control_mode == 3) { - FOC.angle_control_NPID(FOC.target_angle); // 角度无PID控制 - } else if (FOC.control_mode == 4) { - FOC.velocity_open_loop(); // 速度开环控制 + switch (FOC.control_mode) { + case 0: { + break; } - - if (total_us100_counter >= 10) { - // 每1ms执行的代码 - total_us100_counter = 0; // 重置计数器 - + case 1: { + FOC.angle_control(FOC.target_angle); // 角度闭环控制 + break; + } + case 2: { + FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 + break; + } + case 3: { + FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 + break; + } + case 4: { + FOC.velocity_open_loop(); // 速度闭环控制 + break; + } + } + switch (total_us100_counter) { // 每1ms执行的代码，负载均衡 + case 1: { // 设置CAN发送标志 CAN0_transmiter.can0_send_flag = SET; mode = FOC.control_mode; - // 过流检测 - if (sampler.get_current('U') > 2000 || - sampler.get_current('V') > 2000 || - sampler.get_current('W') > 2000) { - FOC.over_current++; - } else { - FOC.over_current = 0; - } - - // 更新各种传感器数据 + break; + } + case 2: { sampler.update_mcu_temp(); + break; + } + case 3: { sampler.update_drv_temp(); + break; + } + case 4: { sampler.update_vbus_voltage(); - - // 停机判断及冷却 - if (FOC.over_current > 10) { - FOC.is_shut_down = 10; - } - + break; + } + } + if (total_us100_counter >= 10) { + total_us100_counter = 0; + // 可以添加每1ms执行的代码，无负载均衡 static u8 ms10_counter = 0; ms10_counter++; + switch (ms10_counter) { // 每10ms执行的代码，负载均衡 + case 1: { + FOC.update_power(); + break; + } + case 2: { + thermalController.update(); + break; + } + } if (ms10_counter >= 10) { ms10_counter = 0; - // 可以添加每10ms执行的代码 - FOC.update_power(); + // 可以添加每10ms执行的代码，无负载均衡 static u8 ms100_counter = 0; ms100_counter++; - if (ms100_counter >= 10) { - ms100_counter = 0; - // 可以添加每100ms执行的代码 + switch (ms100_counter) { // 每100ms执行的代码，负载均衡 + case 1: { + break; + } + if (ms100_counter >= 10) { + ms100_counter = 0; + // 可以添加每100ms执行的代码 - static u8 sec_counter = 0; - sec_counter++; - if (sec_counter >= 10) { - sec_counter = 0; - // 可以添加每1s执行的代码 + static u8 sec_counter = 0; + sec_counter++; + if (sec_counter >= 10) { + sec_counter = 0; + // 可以添加每1s执行的代码 + } } } } @@ -247,11 +270,6 @@ void TIMER3_IRQHandler(void) { static u8 count = 0; count++; - if (FOC.is_shut_down > 0) - FOC.is_shut_down--; - else - FOC.resume(); - // 10ms测试代码 if (count >= 10) { count = 0; @@ -262,7 +280,6 @@ void TIMER3_IRQHandler(void) { if (count_10 >= 10) { count_10 = 0; // 1s测试代码 - FOC.update_power(); } } } diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 83a9d28..0b06313 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -4,6 +4,7 @@ #include "Timer.h" #include "adc.h" #include "ang_sensor.h" +#include "thermal.h" #include "core_cm4.h" #include "initial.h" #include "systick.h" @@ -13,6 +14,7 @@ CAN_transmiter CAN0_transmiter; // CAN初始化 FOC_Controlroller FOC; // FOC控制初始化 sampler sampler; // 采样器初始化 angle_sensor KTH7823; // 角度传感器初始化 +ThermalController thermalController; // 热量控制器初始化 int main(void) { systick_config(); // 系统时钟初始化