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

@@ -91,15 +91,28 @@ class sampler {
 void ADC0_1_IRQHandler(void);
 
 /**
- * @brief ADC1 初始化（VBUS / DRV温度 专用，单通道单次软件触发）
+ * @brief ADC1 初始化：VBUS 连续转换模式（零等待读取）
  * @return void
+ * @note ADC1 上电后即对 ch6(VBUS) 连续转换，软件触发一次后自持续，
+ *       之后读取 DR 寄存器即可拿到最新完成的转换值（无需阻塞等待）。
  */
 void adc1_config(void);
 
+/**
+ * @brief 零等待读取最新 VBUS 原始值（连续模式下任意时刻可调）
+ * @return u16 最近一次完成转换的原始值（陈旧度约 1 个转换周期 ≈1.4μs）
+ * @note 仅主循环任务上下文调用（与其他 ADC1 访问串行，无并发）。
+ *       原理：读 DR 硬件清 EOC，随后下一次完成转换会刷新 DR，
+ *       其间完成的转换被丢弃——每次读到的都是刚完成的样本。
+ */
+u16 adc1_read_vbus_latest(void);
+
 /**
  * @brief ADC1 单次软件触发转换并等待完成（仅主循环任务上下文调用）
- * @param[in] channel ADC 通道号（ADC_CHANNEL_6 / ADC_CHANNEL_9）
+ * @param[in] channel ADC 通道号（如 ADC_CHANNEL_9）
  * @return u16 转换原始值
+ * @note 会临时暂停 VBUS 连续模式、切到目标通道单次转换、再恢复连续，
+ *       全程约 3~5μs，期间 VBUS 读数最多滞后该时长。drv 温度 1kHz 用。
  */
 u16 adc1_read_raw(u8 channel);