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

Robot_V2/device/Can.cpp

@@ -678,7 +678,7 @@ void CAN_transmiter::communication_check() {
 			}
 			target_angle_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 7).f;
-			FOC.target_angle = target_angle_union.f * ANGLE_FACTOR;
+			FOC.target_angle = target_angle_union.f;
 
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 11).f;

Robot_V2/device/Timer.cpp

@@ -167,7 +167,7 @@ void TIMER2_IRQHandler(void) {
 
 		// 增加总计数器 (使用更高效的位运算代替取模运算)
 		total_us100_counter++;
-		system_runtime_us += 100;
+		// system_runtime_us += 100;
 
 		// 每100us执行一次的操作
 		sampler.update_current(); // 更新电流采样值
@@ -175,7 +175,7 @@ void TIMER2_IRQHandler(void) {
 		FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
 
 		if (FOC.control_mode == 1) {
-			FOC.angle_control(FOC.target_angle); // 角度闭环控制
+			FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制
 		} else if (FOC.control_mode == 2) {
 			FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
 		} else if (FOC.control_mode == 4) {

Robot_V2/device/ang_sensor.cpp

@@ -5,6 +5,7 @@
 #include <cstdio>
 #include <cstring>
 #include <stdint.h>
+#include "initial.h"
 
 #define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
 #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
@@ -13,7 +14,6 @@ constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
 constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
 constexpr u16 DEFAULT_COMMAND = 0x0000;
 constexpr u16 FAULT_RECEIVE = 0xFFFF;
-constexpr u16 REDDUCTION_RATIO = 25;
 constexpr float VELOCITY_SCALE = 1e4f;
 constexpr float ANGLE_WRAP_THRESHOLD = 270.0f;
 constexpr float ANGLE_WRAP_VALUE = 360.0f;
@@ -259,7 +259,7 @@ float angle_sensor::update(void) {
  * @return 累积角度值
  */
 float angle_sensor::getAngle(void) const {
-	return m_cumulativeAngle / REDDUCTION_RATIO;
+	return m_cumulativeAngle / REDUCTION_RATIO;
 }
 
 /**
@@ -273,7 +273,7 @@ float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
  * @return 速度值(输出级)
  */
 float angle_sensor::getVelocity(void) const {
-	return getRawVelocity() / REDDUCTION_RATIO;
+	return getRawVelocity() / REDUCTION_RATIO;
 }
 
 /**

Robot_V2/device/ang_sensor.h

@@ -11,7 +11,7 @@ u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
 int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 						 uint16_t *rx_data);
 
-constexpr u16 VEL_FILTER_SIZE = 3;
+constexpr u16 VEL_FILTER_SIZE = 4;
 
 class angle_sensor {
   public:

Robot_V2/sys/initial.h

@@ -8,6 +8,10 @@ extern "C" {
 #include <stdio.h>
 #include "gd32c10x_eval.h"
 
+constexpr u16 REDUCTION_RATIO = 25;
+constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
+constexpr float RAD2DEG = 360.0f / 6.28318530718f;
+
 void rcu_config(void);
 static volatile uint32_t timer_delay_count = 0; // volatile 很重要！
 int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现