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

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

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