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/Can.h

@@ -122,11 +122,6 @@ static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
  * @return u8 返回值
  */
 u8 can_gpio_init(void);				 // CAN 引脚初始化
-/**
- * @brief 配置 CAN 外设
- * @return void
- */
-void can_config(void);				 // CAN 初始化
 /**
  * @brief 配置 CAN FD 外设
  * @return void
@@ -163,16 +158,6 @@ void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发
  */
 void jumpToBOOT();
 
-/**
- * @brief can_loopback_init 函数
- * @return void
- */
-void can_loopback_init(void);
-/**
- * @brief can_loopback_test 函数
- * @return void
- */
-void can_loopback_test(void);
 
 /*!
 	\brief CAN 通信类