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

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

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

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

Robot_V2/device/ang_sensor.h

@@ -11,6 +11,8 @@ 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 = 24;
+
 class angle_sensor {
   public:
 	angle_sensor();
@@ -18,13 +20,17 @@ class angle_sensor {
 	float update(void);
 	void reset(void);
 	float getAngle(void) const;
-
+	float getRawAngle(void) const;
+	float getVelocity(void) const;
+	float getRawVelocity(void) const;
 
   protected:
 	void KTH7823_init(void);
 	bool m_firstUpdate;
 	float m_lastAngle;
 	float m_cumulativeAngle;
+	float m_lastcumulativeAngle;
+	float velocity[VEL_FILTER_SIZE] = {0.0f};
 };
 
 #ifdef __cplusplus