refactor(foc): 重构FOC控制算法与角度传感器接口

- 移除冗余的电机方向和极对数参数传递逻辑
- 简化电气角度计算函数，直接读取角度传感器数据
- 调整电压设置函数，优化PWM输出逻辑
- 注释掉未使用的速度开环控制调用，启用角度闭环控制
- 增加角度和速度获取接口，支持原始值和输出级值区分

feat(sensor): 完善角度传感器数据处理与速度估算
- 增加速度滤波缓冲区，提高速度估算精度
- 改进角度绕回检测阈值，增强鲁棒性
- 添加获取原始角度和速度的接口
- 增加SPI DMA传输超时打印信息

fix(can): 修正CAN通信中的数据打包与解析问题
- 正确发送电机实时速度数据
- 修正CAN FD数据包长度以匹配实际内容
- 角度目标值增加缩放因子适配外部控制指令

Robot_V2/Algorithm/DengFOC.h

@@ -1,6 +1,7 @@
 // 函数声明
 #ifndef _DENGFOC_H
 #define _DENGFOC_H
+#include "pid.h"
 #include <stdint.h>
 class FOC_Controlroller {
   public:
@@ -18,13 +19,16 @@ class FOC_Controlroller {
 	u8 voltage_error = 0;
 	u8 is_shut_down = 0;
 
-	float velocity_open_loop(float target_velocity);
+	float velocity_open_loop();
+	float angle_control(float target_angle);
+	// void SET_VEL_PID(float P, float I, float D, float ramp);   // 速度环PID设置
+	// void SET_ANGLE_PID(float P, float I, float D, float ramp); // 角度环PID设置
+
 	void update_power(void);
-	void DFOC_alignSensor(int _PP, int _DIR);
+	void DFOC_alignSensor(int _PP);
 	void shut_down(void);
 	void resume(void);
 
-
   protected:
 	std::u64 last_time_us = 0;
 	std::u64 now_us = 0;
@@ -33,17 +37,16 @@ class FOC_Controlroller {
 	float zero_electric_angle = 0;
 	float voltage_power_supply = 24.0f; // 电源电压
 
-	void driver_output(float Ua, float Ub, float Uc);
+	PIDController vel_loop =
+		PIDController{2, 0, 0, 100000, voltage_power_supply / 2};
+	PIDController angle_loop = PIDController{2, 0, 0, 100000, 100};
+
 	void setPhaseVoltage(float Uq, float Ud, float angle_el);
-	void setTorque(float Uq,float angle_el);
-	float DFOC_M0_Current();
-	void DFOC_M0_setTorque(float Target);
 };
 
 float _normalizeAngle(float angle);
 void DFOC_Vbus(float power_supply);
-float _electricalAngle(float shaft_angle);
-float DFOC_M0_Angle();
-float serial_motor_target();
+inline float _electricalAngle();
+float DFOC_M0_Angle(void);
 
 #endif