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/Algorithm/pid.h

@@ -15,9 +15,15 @@ class PIDController {
 	PIDController(float P, float I, float D, float ramp, float limit);
 	~PIDController() = default;
 
-	float operator()(float error);
 	/**
-	 * @brief 设置 set_PID 对应的参数或状态
+	 * @brief PID 计算（唯一入口）
+	 * @param[in] error 控制误差
+	 * @param[in] measurement 反馈测量量（D 项对它微分，目标阶跃不产生微分踢）
+	 * @return float PID 输出
+	 */
+	float operator()(float error, float measurement);
+	/**
+	 * @brief 设置 set_PID 对应的参数
 	 * @param[in] P 参数
 	 * @param[in] I 参数
 	 * @param[in] D 参数
@@ -25,13 +31,13 @@ class PIDController {
 	 */
 	void set_PID(float P, float I, float D);
 	/**
-	 * @brief 设置 set_P 对应的参数或状态
+	 * @brief 设置 set_P 对应的参数
 	 * @param[in] pp 参数
 	 * @return void
 	 */
 	void set_P(float pp);
 	/**
-	 * @brief 设置 set_limit 对应的参数或状态
+	 * @brief 设置 set_limit 对应的参数
 	 * @param[in] limit 参数
 	 * @return void
 	 */
@@ -46,6 +52,13 @@ class PIDController {
 	 * @return void
 	 */
 	void reset_integral(); // 仅清除积分项，保留输出斜坡状态
+	/**
+	 * @brief 外部饱和泄放积分（反算抗饱和的补充入口）
+	 * @param[in] excess 输出超出限幅的部分（带符号），如前馈叠加后超限
+	 * @return void
+	 * @note 调用方向须与 PID 输出到最终 Uq 的符号一致（有 ×DIR 的要乘上 DIR）
+	 */
+	void bleed_integral(float excess);
 
 	float P; //!< 比例增益(P环增益)
 	float I; //!< 积分增益（I环增益）
@@ -54,11 +67,20 @@ class PIDController {
 	float limit;
 
   protected:
+	/**
+	 * @brief 公共计算路径：积分、抗饱和、输出斜坡
+	 * @param[in] error 控制误差
+	 * @param[in] derivative 已完成低通滤波的微分项（D 已乘入）
+	 * @return float PID 输出
+	 */
+	float step(float error, float derivative);
+
 	float error_prev;			  //!< 最后的跟踪误差值
 	float output_prev;			  //!< 最后一个 pid 输出值
 	float integral_prev;		  //!< 最后一个积分分量值
-	unsigned long timestamp_prev; //!< 上次执行时间戳
-	float integral_separation_threshold;  //!< 积分分离阈值，用于抗积分饱和
+	float measurement_prev;		  //!< 上次测量值（对测量微分用）
+	float derivative_filt;		  //!< 微分项一阶低通滤波后的变化率
+	bool deriv_init;			  //!< 测量微分是否已完成首次采样
 };
 
 #endif