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