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

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

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,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();
 			}
 		}
 	}