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

- 移除冗余的三值约束宏定义 `_constrain3`
- 修改关机逻辑，引入 `thermal_uncontrol` 标志替代旧 shut_down 计数方式
- 调整恢复函数中错误标志位清零逻辑
- 将部分 uint8_t 类型成员变量改为 bool 类型以提高可读性
- 在类中添加电源电压成员变量并调整 PID 参数初始化
- 移除 PID 控制器中不必要的 CAN 测试代码
- 添加 thermal 控制相关文件引用及初始化
- 使用 switch 替代多重 if-else 提高控制模式处理效率
- 引入基于定时器的任务负载均衡调度机制
- 初始化热量控制器并在主循环中调用更新逻辑

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);

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};
 

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;

Robot_V2/Algorithm/thermal.cpp

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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,51 +176,71 @@ 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();
-			sampler.update_drv_temp();
-			sampler.update_vbus_voltage();
-
-			// 停机判断及冷却
-			if (FOC.over_current > 10) {
-				FOC.is_shut_down = 10;
+			break;
 		}
-
+		case 3: {
+			sampler.update_drv_temp();
+			break;
+		}
+		case 4: {
+			sampler.update_vbus_voltage();
+			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++;
+				switch (ms100_counter) { // 每100ms执行的代码，负载均衡
+				case 1: {
+					break;
+				}
 					if (ms100_counter >= 10) {
 						ms100_counter = 0;
 						// 可以添加每100ms执行的代码
@@ -234,6 +256,7 @@ void TIMER2_IRQHandler(void) {
 			}
 		}
 	}
+}
 
 /**
  * @brief   TIMER3中断处理函数
@@ -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

@@ -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();	 // 系统时钟初始化