feat(FOC): 实现电流环控制并优化相关逻辑

新增了电流环控制功能，包括通过三相电流计算 Iq 的 `cal_Iq_Id` 方法，并在 `current_control` 中实现了基于 PID 的电流控制逻辑。同时对电压设置、占空比计算等部分进行了调整与优化。

refactor(filter): 移除冗余的电流卡尔曼滤波器实现
删除了 `Filter.cpp` 和 `Filter.h` 中关于 `CurrentKalman` 类的冗余代码，统一外部定义，简化结构。

refactor(health): 改进热量控制器算法和数据类型
将热累积计算改为更精确的分段处理方式，并引入 Q16.16 定点数提升运算精度与性能；增加对异常温度值的判断修正。

fix(CAN): 调整电流发送顺序及标志位设置位置
修改 CAN 发送中 V 相与 W 相电流打包顺序，确保通信正确性；调整发送标志设置逻辑至主循环任务内触发。

chore(headers): 清理无用头文件引用和结构调整
移除了多个源文件中不必要的头文件包含（如 `<cstdio>`、`<array>`），并对部分全局变量声明做了 extern 处理以避免重复定义问题。

Robot_V2/Algorithm/DengFOC.cpp

@@ -1,6 +1,5 @@
 #include "DengFOC.h"
 #include "Can.h"
-#include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
 #include "ang_sensor.h"
@@ -27,9 +26,6 @@ extern sampler sampler;
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
 
-CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
-CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
-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)))
@@ -47,6 +43,27 @@ inline float _normalizeAngle(float angle) {
 	// 取余运算可以用于归一化，列出特殊值例子算便知
 	return a >= 0 ? a : (a + 2 * PI);
 }
+
+// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
+inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
+										  float current_c) {
+	// 计算电气角度
+	float angle_el = _normalizeAngle(shaft_angle * PP);
+
+	// Clarke变换 (使用三相电流)
+	float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
+					(1.0f / 3.0f) * current_c;
+	float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
+
+	// Park变换
+	float sin_el, cos_el;
+	cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
+	sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
+	float I_q = I_beta * cos_el - I_alpha * sin_el;
+
+	return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA
+}
+
 /**
  * @brief 电机控制类构造函数
  *
@@ -77,15 +94,15 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
 	Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
 
 	// 计算占空比并限制范围
-	float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
-	float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
-	float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
+	duty_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
+	duty_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
+	duty_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
 	// 设置PWM输出
 	float voltage_exchange = limit_voltage / v_safe;
-	dc_a = dc_a * voltage_exchange;
-	dc_b = dc_b * voltage_exchange;
-	dc_c = dc_c * voltage_exchange;
-	Set_3Pwm(dc_a, dc_b, dc_c);
+	duty_a = duty_a * voltage_exchange;
+	duty_b = duty_b * voltage_exchange;
+	duty_c = duty_c * voltage_exchange;
+	Set_3Pwm(duty_a, duty_b, duty_c);
 	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
 
@@ -94,7 +111,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
  *
  */
 void FOC_Controlroller::shut_down() {
-	thermal_uncontrol = 0;
 	is_shut_down = 1;
 	gpio_bit_reset(GPIOB, GPIO_PIN_12);
 }
@@ -119,7 +135,7 @@ void FOC_Controlroller::drv_hold(float duty) {
  *
  */
 void FOC_Controlroller::resume() {
-	thermal_uncontrol = false;
+	over_current = false;
 	voltage_error = false;
 	is_shut_down = false;
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
@@ -155,6 +171,8 @@ float FOC_Controlroller::velocity_open_loop() {
 	const float Uq = voltage_power_supply / 2.0f;
 
 	setPhaseVoltage(Uq, 0.0f);
+	CAN0_transmiter.testfloat_union.f =
+		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
 
 	return limit_voltage;
 }
@@ -223,5 +241,19 @@ float FOC_Controlroller::velocity_control(float target_velocity) {
 	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
+	CAN0_transmiter.testfloat_union.f =
+		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
 	return rt_velocity;
+}
+
+float FOC_Controlroller::current_control(float target_current) {
+	// PID 控制计算
+	float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
+	float Uq = current_loop(target_current - Iq);
+	CAN0_transmiter.testfloat_union.f = Iq;
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f);
+	return 1;
 }