cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(FOC): 优化FOC控制逻辑与热管理机制

Browse Source 
- 移除冗余的三值约束宏定义 `_constrain3`
- 修改关机逻辑，引入 `thermal_uncontrol` 标志替代旧 shut_down 计数方式
- 调整恢复函数中错误标志位清零逻辑
- 将部分 uint8_t 类型成员变量改为 bool 类型以提高可读性
- 在类中添加电源电压成员变量并调整 PID 参数初始化
- 移除 PID 控制器中不必要的 CAN 测试代码
- 添加 thermal 控制相关文件引用及初始化
- 使用 switch 替代多重 if-else 提高控制模式处理效率
- 引入基于定时器的任务负载均衡调度机制
- 初始化热量控制器并在主循环中调用更新逻辑
This commit is contained in:
cbb
2025-11-12 00:46:26 +08:00
parent 7a602dda48
commit db2d08032c
9 changed files with 145 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+3 -8
View File
@@ -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);
Robot_V2/Algorithm/DengFOC.h
+6 -7
View File
@@ -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};
Robot_V2/Algorithm/pid.cpp
-2
View File
@@ -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;
Robot_V2/Algorithm/thermal.cpp
+42
View File
@@ -0,0 +1,42 @@
#include "thermal.h"
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
#include <cstdio>
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;
}
Robot_V2/Algorithm/thermal.h
+27
View File
@@ -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
Robot_V2/Robot_V2.cproject.yml
+2
View File
@@ -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
Robot_V2/device/Can.cpp
-2
View File
@@ -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;
Robot_V2/device/Timer.cpp
+63 -46
View File
@@ -10,6 +10,7 @@
#include "gd32c10x_misc.h"
#include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h"
#include "thermal.h"
#include <cstdio>
#include <stdbool.h>
#include <stdint.h>
@@ -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();
}
}
}
Robot_V2/main.cpp
+2
View File
@@ -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(); // 系统时钟初始化