feat(FOC): 优化FOC控制算法并修复多个控制环路问题

- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值
  以避免重复三角函数计算，提升性能约15-300周期
- 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的
  震荡问题和正转正常反转震荡的根本原因
- 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦
  hunting抑制效果
- 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待，
  提升实时性能
- 修复初始化期间电压分母为零的问题，添加下限保护防止P增益
  无穷大
- 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击
- 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定
  性能

perf(Filter): 优化中值滤波算法性能

- 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络，
  用12次比较替代通用std::nth_element算法，显著提升性能
- 保留通用回退路径处理其他尺寸情况

Robot_V2/device/ang_sensor.h

@@ -21,14 +21,17 @@ extern "C" {
  */
 u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
 /**
- * @brief spi_rw_half_word_dma 函数
- * @param[in] spi_periph 参数
- * @param[in] tx_data 参数
- * @param[in] rx_data 参数
- * @return int 返回值
+ * @brief spi_rw_half_word_polling 函数
+ * @param[in] spi_periph SPI 外设
+ * @param[in] tx_data 发送的半字
+ * @param[out] rx_data 接收数据输出指针
+ * @return int 0 成功，-1 超时
+ * @note 单个半字收发：直接寄存器轮询。原 DMA 实现每次传输要重配两个
+ *       DMA 通道并触发两次 FTF 中断，之后 CPU 仍忙等完成——对单半字
+ *       传输 DMA 只有开销没有收益（20kHz 调用下约 40k 次/秒额外中断）。
  */
-int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
-						 uint16_t *rx_data);
+int spi_rw_half_word_polling(uint32_t spi_periph, uint16_t tx_data,
+							 uint16_t *rx_data);
 
 
 /**
@@ -91,8 +94,6 @@ class angle_sensor {
 	float current_angle_rad;
 	bool m_firstUpdate;
 	float m_lastAngle;
-	float m_lastcumulativeAngle;
-	float velocity[VEL_FILTER_SIZE] = {0.0f};
 };
 /**
  * @brief 快速角度归一化到 [0, 360)