feat(FOC): 优化SVPWM算法并重构角度控制逻辑

- 引入新的三值约束宏 `_constrain3` 用于SVPWM计算优化
- 修改 `setPhaseVoltage` 函数签名，移除冗余的角度参数，内部自动计算电气角度
- 调整PWM占空比计算方式，提高电压利用率和控制精度
- 移除旧的DEG2RAD和RAD2DEG常量定义，改由initial.h统一管理
- 在initial.h中新增减速比常量REDUCTION_RATIO，并修正拼写错误
- 为角度传感器类更新速度滤波器尺寸，提升速度估算稳定性
- 更新PID控制器时间片常量，简化dt计算逻辑，提高执行效率
- 切换角度闭环控制至新NPID版本，增强系统响应性能
- 修正CAN通信中的目标角度解析逻辑，确保指令正确应用

Robot_V2/Algorithm/DengFOC.cpp

@@ -5,6 +5,7 @@
 #include "adc.h"
 #include "ang_sensor.h"
 #include "gd32c10x_dbg.h"
+#include "initial.h"
 #include "pid.h"
 #include "systick.h"
 #include <cmath>
@@ -13,8 +14,6 @@
 #include <stdint.h>
 
 constexpr double PI = 3.1415926535897932384626433832795f;
-constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
-constexpr float RAD2DEG = 360.0f / 6.28318530718f;
 constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
 constexpr float _1_SQRT3 = 0.57735026919f;
 constexpr float _2_SQRT3 = 1.15470053838f;
@@ -34,6 +33,11 @@ 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)))
+
+// 三值约束比较函数，用于SVPWM算法
+#define _constrain3(a, b, c)                                                   \
+	((a) < (b) ? ((a) < (c) ? (a) : (c)) : ((b) < (c) ? (b) : (c)))
+
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
@@ -59,14 +63,15 @@ inline float _electricalAngle() {
 FOC_Controlroller::FOC_Controlroller() { ; }
 
 /**
- * @brief 设置电机输出
+ * @brief 设置电机输出，使用优化的SVPWM算法
  *
  * @param Uq: q轴电压
  * @param Ud: d轴电压
  * @param angle_el: 电机轴角度
  */
-inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
-											   float angle_el) {
+inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
+	// 计算电气角度
+	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
 	// 帕克逆变换 (直接计算，避免函数调用开销)
 	const float sin_el = sin(angle_el);
 	const float cos_el = cos(angle_el);
@@ -79,15 +84,17 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 	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);
 	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
 	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
 	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
+	// 设置PWM输出
+	dc_a = dc_a * limit_voltage / voltage_power_supply;
+	dc_b = dc_b * limit_voltage / voltage_power_supply;
+	dc_c = dc_c * limit_voltage / voltage_power_supply;
 	Set_3Pwm(dc_a, dc_b, dc_c);
 	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
+
 /**
  * @brief 电机关机
  *
@@ -128,13 +135,10 @@ float FOC_Controlroller::velocity_open_loop() {
 	shaft_angle =
 		_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
 
-	// 直接计算电气角度，避免函数调用开销
-	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
-
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
 
-	setPhaseVoltage(Uq, 0.0f, angle_el);
+	setPhaseVoltage(Uq, 0.0f);
 
 	return limit_voltage;
 }
@@ -157,19 +161,35 @@ float FOC_Controlroller::velocity_open_loop() {
  * @brief 角度闭环控制
  *
  * @param target_angle: 目标角度
- * @return float: q轴电压
+ * @return float: 当前角度
  */
 float FOC_Controlroller::angle_control(float target_angle) {
 	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
-	static float Kp = 2.0f;
-	float Uq =
-		1.0f - _constrain(Kp * (target_angle - DIR * shaft_angle) * RAD2DEG,
-						  -voltage_power_supply, voltage_power_supply);
-	static float Ud = 0.0f;
-	// 直接计算电气角度，避免函数调用开销
-	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
+	// PID 控制计算
+	float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR);
+	CAN0_transmiter.testfloat_union.f =
+		angle_loop(target_angle * ANGLE_FACTOR - shaft_angle);
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f, angle_el);
+	setPhaseVoltage(Uq, 0.0f);
+	return shaft_angle;
+}
+
+/**
+ * @brief 角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @return float: 当前角度
+ */
+float FOC_Controlroller::angle_control_NPID(float target_angle) {
+	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
+	float Kp = 20.0f;
+	// 控制力矩计算
+	float Uq =
+		1.0f -
+		_constrain(Kp * (target_angle * ANGLE_FACTOR - DIR * shaft_angle),
+				   -voltage_power_supply, voltage_power_supply);
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
 }
 
@@ -177,16 +197,15 @@ float FOC_Controlroller::angle_control(float target_angle) {
  * @brief 速度闭环控制
  *
  * @param target_velocity: 目标速度
- * @return float: q轴电压
+ * @return float: 当前速度
  */
 float FOC_Controlroller::velocity_control(float target_velocity) {
-	// PID 控制计算
-	float Uq = v_half - vel_loop(target_velocity - rt_velocity);
-
 	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
-	// 直接计算电气角度，避免函数调用开销
-	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
+	// PID 控制计算
+	float Uq = vel_loop(rt_velocity - target_velocity);
+
+	CAN0_transmiter.testfloat_union.f = Uq;
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f, angle_el);
+	setPhaseVoltage(Uq, 0.0f);
 	return rt_velocity;
 }

Robot_V2/Algorithm/DengFOC.h

@@ -22,6 +22,7 @@ class FOC_Controlroller {
 	float velocity_open_loop();
 	float angle_control(float target_angle);
 	float velocity_control(float target_velocity);
+	float angle_control_NPID(float target_angle);
 
 	void update_power(void);
 	void DFOC_alignSensor(int _PP);
@@ -38,10 +39,10 @@ class FOC_Controlroller {
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
 	PIDController vel_loop =
-		PIDController{0.8f, 3.0f, 0, 100000, voltage_power_supply};
-	PIDController angle_loop = PIDController{2, 0, 0, 100000, 100};
+		PIDController{1.5f, 1.0f, 0.0005, 100000, voltage_power_supply};
+	PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 100000, voltage_power_supply};
 
-	void setPhaseVoltage(float Uq, float Ud, float angle_el);
+	void setPhaseVoltage(float Uq, float Ud);
 };
 
 float _normalizeAngle(float angle);

Robot_V2/Algorithm/pid.cpp

@@ -5,48 +5,41 @@
 #include "Can.h"
 
 extern CAN_transmiter CAN0_transmiter;
+constexpr float TIME_SLICE = 1e-4;
 
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
 	  ,
 	  limit(limit) // PID控制器输出限幅
 	  ,
-	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {
-	timestamp_prev = get_system_runtime_us();
-}
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
 
 // PID 控制器函数
 float PIDController::operator()(float error) {
-	// 计算两次循环中间的间隔时间
-	unsigned long timestamp_now = get_system_runtime_us();
-	float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
-	if (Ts <= 0 || Ts > 0.5f)
-		Ts = 1e-3f;
-
 	// P环
 	float proportional = P * error;
 	// Tustin 散点积分（I环）
-	float integral = integral_prev + I * Ts * 0.5f * (error + error_prev);
+	float integral =
+		integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
 	integral = _constrain(integral, -limit, limit);
 	// D环（微分环节）
-	float derivative = D * (error - error_prev) / Ts;
+	float derivative = D * (error - error_prev) / TIME_SLICE;
 	// 将P,I,D三环的计算值加起来
 	float output = proportional + integral + derivative;
 	output = _constrain(output, -limit, limit);
 
 	if (output_ramp > 0) {
 		// 对PID的变化速率进行限制
-		float output_rate = (output - output_prev) / Ts;
+		float output_rate = (output - output_prev) / TIME_SLICE;
 		if (output_rate > output_ramp)
-			output = output_prev + output_ramp * Ts;
+			output = output_prev + output_ramp * TIME_SLICE;
 		else if (output_rate < -output_ramp)
-			output = output_prev - output_ramp * Ts;
+			output = output_prev - output_ramp * TIME_SLICE;
 	}
 	// 保存值（为了下一次循环）
 	integral_prev = integral;
 	output_prev = output;
 	error_prev = error;
-	timestamp_prev = timestamp_now;
 
 	return output;
 }