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

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

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