```

feat(DengFOC):
新增了基于三相电流采样的dq轴电流计算函数`cal_Iq_Id`
调整了开环控制中的电压限制逻辑
更新了默认电源电压值为24V。
KHT7823测试代码
```

Robot_V2/Algorithm/DengFOC.cpp

@@ -1,23 +1,41 @@
 #include "DengFOC.h"
+#include "Filter.h"
 #include "Timer.h"
+#include "adc.h"
+#include "pid.h"
 #include "systick.h"
 #include <cmath>
 #include <cstdio>
 #include <gd32c10x.h>
 #include <stdint.h>
 
-
 constexpr double PI = 3.1415926535897932384626433832795f;
+constexpr float _1_SQRT3 = 0.57735026919f;
+constexpr float _2_SQRT3 = 1.15470053838f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11, DIR = 1;
 constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
+constexpr float zero_electric_angle = 0.0f;
+
+extern sampler sampler;
+
+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);
+PIDController vel_loop_M0 = PIDController{2, 0, 0, 100000, 24.0 / 2.0};
+PIDController angle_loop_M0 = PIDController{2, 0, 0, 100000, 100};
+PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800};
+
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
+inline float _electricalAngle(float shaft_angle) {
+	return _normalizeAngle((float)(DIR * PP) * shaft_angle -
+						   zero_electric_angle);
+}
 
 /**
  * @brief 归一化角度
@@ -90,7 +108,7 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
 	angle_el = _normalizeAngle(angle_el);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
-	const float Uq = limit_voltage;
+	const float Uq = voltage_power_supply / 2.0f;
 	const float Ud = 0.0f;
 
 	// 帕克逆变换 (直接计算，避免函数调用开销)
@@ -106,9 +124,9 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
 	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
 
 	// 计算占空比
-	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
+	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
+	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
+	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
 	const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
 	const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
 	const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
@@ -141,4 +159,60 @@ void FOC_Controlroller::shut_down() {
 void FOC_Controlroller::resume() {
 	is_shut_down = 0;
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
-}
+}
+
+// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
+float cal_Iq_Id(float current_a, float current_b, float angle_el) {
+	float I_alpha = current_a;
+	float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
+
+	float ct = cos(angle_el);
+	float st = sin(angle_el);
+	// float I_d = I_alpha * ct + I_beta * st;
+	float I_q = I_beta * ct - I_alpha * st;
+	return I_q;
+}
+
+void FOC_Controlroller::setTorque(float Uq, float angle_el) {
+	Uq =
+		_constrain(Uq, -(voltage_power_supply) / 2, (voltage_power_supply) / 2);
+	float Ud = 0;
+	angle_el = _normalizeAngle(angle_el);
+	// 帕克逆变换
+	Ualpha = -Uq * sin(angle_el);
+	Ubeta = Uq * cos(angle_el);
+
+	// 克拉克逆变换
+	Ua = Ualpha + voltage_power_supply / 2;
+	Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
+	Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
+	driver_output(Ua, Ub, Uc);
+}
+
+float FOC_Controlroller::DFOC_M0_Current() {
+	// 使用卡尔曼滤波后的三相电流计算Iq
+	float Iu_filtered = curU_kalman.update(sampler.get_current('U'), Ua);
+	float Iv_filtered = curV_kalman.update(sampler.get_current('V'), Ub);
+	float Iw_filtered = curW_kalman.update(sampler.get_current('W'), Uc);
+
+	// 根据基尔霍夫电流定律验证三相电流总和是否接近0
+	// float current_sum = Iu_filtered + Iv_filtered + Iw_filtered;
+
+	// 用三相滤波后的电流计算Iq（通过Clarke和Park变换）
+	// 方法1: 使用U和V相计算
+	float I_q_M0_ori =
+		cal_Iq_Id(Iu_filtered, Iv_filtered, _electricalAngle(shaft_angle));
+
+	// 方法2: 也可以使用其他两相的组合计算，然后取平均值以提高精度
+	// float I_q_UW = cal_Iq_Id(Iu_filtered, Iw_filtered,
+	// _electricalAngle(shaft_angle)); float I_q_VW = cal_Iq_Id(Iv_filtered,
+	// Iw_filtered, _electricalAngle(shaft_angle)); float I_q_M0_ori = (I_q_UV +
+	// I_q_UW + I_q_VW) / 3.0f;
+
+	return I_q_M0_ori;
+	// return 0;
+}
+void FOC_Controlroller::DFOC_M0_setTorque(float Target) // 电流力矩环
+{
+	//   setTorque(current_loop_M0(Target-DFOC_M0_Current()),_electricalAngle());
+}