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

Robot_V2/Algorithm/DengFOC.h

@@ -5,7 +5,7 @@
 #include <stdint.h>
 
 class FOC_Controlroller {
-	
+
   public:
 	FOC_Controlroller();
 
@@ -16,14 +16,17 @@ class FOC_Controlroller {
 	float target_velocity = 0.0f;
 	float rt_angle = 0.0f;
 	float rt_velocity = 0.0f;
-	bool thermal_uncontrol = false;
+	bool over_current = false;
 	bool voltage_error = false;
 	bool is_shut_down = false;
 	float voltage_power_supply = 24.0f; // 电源电压
-
+	float v_safe = voltage_power_supply * 1.05f;
+	float duty_a = 0, duty_b = 0, duty_c = 0;
+	float Ua = 0, Ub = 0, Uc = 0;
 	float velocity_open_loop();
 	float angle_control(float target_angle);
 	float velocity_control(float target_velocity);
+	float current_control(float target_current);
 	float angle_control_NPID(float target_angle);
 
 	void update_power(void);
@@ -34,31 +37,25 @@ class FOC_Controlroller {
 	void drv_hold(float duty = 0.5f);
 
   protected:
-	std::u64 last_time_us = 0;
-	std::u64 now_us = 0;
 	float shaft_angle = 0;
-	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
+	float Ualpha, Ubeta = 0;
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	float v_safe = voltage_power_supply * 0.95f;
 
-	// PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000,
-	// voltage_power_supply};//8V
-	// PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000,
-	// 										 voltage_power_supply}; // 8V
+
 	PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
 										   voltage_power_supply}; // 12V
 	PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
 											 voltage_power_supply}; // 12V
+	PIDController current_loop =
+		PIDController{0.01f, 0.001f, 0, 100000, voltage_power_supply}; // 12V
 
 	void setPhaseVoltage(float Uq, float Ud);
+	float cal_Iq_Id(float current_a, float current_b, float current_c);
 };
 
 float _normalizeAngle(float angle);
-void DFOC_Vbus(float power_supply);
 inline float _electricalAngle();
-float DFOC_M0_Angle(void);
-
 
 constexpr u16 LUT_SIZE = 1024;
 constexpr u16 LUT_MASK = (LUT_SIZE - 1);

Robot_V2/Algorithm/Filter.cpp

@@ -1,13 +1,15 @@
 #include "Filter.h"
 #include "Can.h"
 #include "ang_sensor.h"
-#include "gd32c10x_can.h"
 #include <algorithm>
 #include <cstring>
 #include <stdint.h>
 
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
+EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
+EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
+
 constexpr u8 SIZE_FLOAT = sizeof(float);
 
 /*!
@@ -28,56 +30,6 @@ float medium_filter(float values[], u8 size) {
 	return sorted[size / 2]; // 返回中值
 }
 
-// CurrentKalman类的实现
-/*!
-	\brief      构造函数，初始化卡尔曼滤波器参数
-	\param[in]  initial_current 初始电流值
-	\param[in]  L 电机电感(H)	0.0026H
-	\param[in]  R 电机电阻(Ω)	4.8 Ω
-	\param[in]  T 控制周期(s)	1e -4
-	\param[in]  Q 过程噪声协方差(典型值: 1e-6 ~ 1e-4)
-	\param[in]  R_noise 测量噪声协方差(典型值: 电流范围的0.5%~2%)
-	\retval     none
-*/
-CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
-							 float Q, float R_noise)
-	: kalman(initial_current, 1.0f, Q, R_noise), L(L), R_m(R), T(T) {}
-/*!
-	\brief      更新卡尔曼滤波器状态
-	\param[in]  measured_value 测量值
-	\param[in]  applied_voltage 应用电压值
-	\retval     估计的电流值
-	\note       实现卡尔曼滤波的预测和更新步骤
-*/
-float CurrentKalman::update(float measured_value, float applied_voltage) {
-	// 1. 预测步骤 - 基于电机模型
-	float predicted_current = kalman.getState() + T * (applied_voltage - R_m * kalman.getState()) / L;
-	
-	// 2. 预测误差协方差已经在kalman.predict中处理
-	
-	// 3. 执行预测
-	kalman.predict(predicted_current);
-	
-	// 4. 更新步骤 - 直接使用测量值更新
-	return kalman.update(measured_value);
-}
-/*!
-	\brief      获取当前电流估计值
-	\retval     当前电流估计值
-*/
-float CurrentKalman::getCurrent() const { return kalman.getState(); }
-/*!
-	\brief      设置过程噪声协方差
-	\param[in]  q 过程噪声协方差值
-	\retval     none
-*/
-void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
-/*!
-	\brief      设置测量噪声协方差
-	\param[in]  r 测量噪声协方差值
-	\retval     none
-*/
-void CurrentKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); }
 
 // EncoderVelocityKalman类的实现
 /*!
@@ -109,7 +61,7 @@ float EncoderVelocityKalman::update(float measured_velocity) {
 
 /*!
 	\brief      获取当前速度估计值
-	\retval     当前速度估计值 (rad/s)
+	\retval     当前速度估计值 (deg/s)
 */
 float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); }
 
@@ -158,9 +110,8 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
 float EncoderAngleKalman::update(float measured_angle) {
 	// 更新last_angle以保持同步
 	last_angle = kalman.getState();
-	// 1. 预测步骤 - 假设角度符合随机游走模型（角速度为0）
-	//    由于角度变化相对缓慢，我们可以简单预测角度保持不变
-	kalman.predict(kalman.getState());
+	// 1. 预测步骤 - 速度估计
+	kalman.predict(last_angle);
 
 	// 2. 更新步骤 - 使用处理后的角度值更新
 	float filtered_angle = kalman.update(measured_angle);

Robot_V2/Algorithm/Filter.h

@@ -1,7 +1,6 @@
 #ifndef _FILTER_H
 #define _FILTER_H
 
-#include <cstdio>
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -14,7 +13,6 @@ extern "C" {
 #endif
 
 #ifdef __cplusplus
-#include <array>
 #include <cstring>
 #include <stdint.h>
 
@@ -26,7 +24,7 @@ float medium_filter(float values[], u8 size);
 */
 template <typename T> class KalmanFilter {
   private:
-	T x; // 状态估计
+	T xp; // 状态估计
 	T P; // 估计误差协方差
 	T Q; // 过程噪声协方差
 	T R; // 测量噪声协方差
@@ -41,7 +39,7 @@ template <typename T> class KalmanFilter {
 	*/
 	KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
 				 T measurement_noise)
-		: x(initial_state), P(initial_error_covariance), Q(process_noise),
+		: xp(initial_state), P(initial_error_covariance), Q(process_noise),
 		  R(measurement_noise) {}
 
 	/*!
@@ -50,7 +48,7 @@ template <typename T> class KalmanFilter {
 		\retval none
 	*/
 	void predict(T state_prediction) {
-		x = state_prediction;
+		xp = state_prediction;
 		P += Q;
 	}
 
@@ -65,10 +63,10 @@ template <typename T> class KalmanFilter {
 		T S = P + R;								// 残差协方差
 		T K = P / S;								// 卡尔曼增益
 
-		x += K * y;		 // 更新状态估计
+		xp += K * y;		 // 更新状态估计
 		P = (1 - K) * P; // 更新误差协方差
 
-		return x;
+		return xp;
 	}
 
 	/*!
@@ -79,16 +77,16 @@ template <typename T> class KalmanFilter {
 	T update(T measurement) {
 		T K = P / (P + R); // 卡尔曼增益
 
-		x += K * (measurement - x); // 更新状态估计
+		xp += K * (measurement - xp); // 更新状态估计
 		P = (1 - K) * P;			// 更新误差协方差
-		return x;
+		return xp;
 	}
 
 	/*!
 		\brief 获取当前状态估计值
 		\retval 当前状态估计值
 	*/
-	T getState() const { return x; }
+	T getState() const { return xp; }
 
 	/*!
 		\brief 设置过程噪声协方差
@@ -105,34 +103,6 @@ template <typename T> class KalmanFilter {
 	void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
 };
 
-/*!
-	\brief 电流卡尔曼滤波器类
-	\note  基于电机模型的卡尔曼滤波器实现，用于电流状态估计
-	\note
-	// 创建卡尔曼滤波器实例
-	CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.02f, 1e-6f, 0.02f);
-	// 更新滤波器状态并获取估计值
-	float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage);
-*/
-class CurrentKalman {
-  private:
-	KalmanFilter<float> kalman;
-	float L;   // 电机电感(H)
-	float R_m; // 电机电阻(Ω)
-	float T;   // 控制周期(s)
-
-  public:
-	CurrentKalman(float initial_current, float L, float R, float T, float Q,
-				  float R_noise);
-
-	float update(float measured_current, float applied_voltage);
-
-	float getCurrent() const;
-
-	void setProcessNoise(float q);
-
-	void setMeasurementNoise(float r);
-};
 
 /*!
 	\brief 编码器速度卡尔曼滤波器类
@@ -182,6 +152,7 @@ public:
 	void setMeasurementNoise(float r);
 };
 
+
 #endif // __cplusplus
 
 #endif // _FILTER_H