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

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

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: