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.cpp

@@ -12,33 +12,55 @@
 #include <cmath>
 
 extern CAN_transmiter CAN0_transmiter;
+// 微分项一阶低通离散系数：α = Ts / (τ + Ts)
+constexpr float DERIV_LPF_ALPHA = TIME_SLICE / (DERIV_LPF_TAU + TIME_SLICE);
+
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
 	  ,
 	  limit(limit) // PID控制器输出限幅
 	  ,
-	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f),
+	  measurement_prev(0.0f), derivative_filt(0.0f), deriv_init(false) {}
 
-// PID 控制器函数
-float PIDController::operator()(float error) {
+// 测量微分版本：D 项对测量量而非误差微分，目标阶跃不再产生微分踢
+float PIDController::operator()(float error, float measurement) {
+	// 首次调用只建立测量基线，避免 (meas-0)/Ts 的假微分尖峰
+	if (!deriv_init) {
+		measurement_prev = measurement;
+		deriv_init = true;
+	}
+	float d_raw = -(measurement - measurement_prev) / TIME_SLICE;
+	derivative_filt += DERIV_LPF_ALPHA * (d_raw - derivative_filt);
+	measurement_prev = measurement;
+	return step(error, D * derivative_filt);
+}
+
+// 公共计算路径：Tustin 积分 + 条件积分抗饱和 + 输出斜坡
+float PIDController::step(float error, float derivative) {
 	// P环
 	float proportional = P * error;
 
 	// Tustin 散点积分（I环）
-	// 添加积分限幅，防止积分饱和
 	float integral =
 		integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
+
+	// 将P,I,D三环的计算值加起来，与限幅比较得到未饱和/饱和输出
+	float output_unsat = proportional + integral + derivative;
+	float output = _constrain(output_unsat, -limit, limit);
+
+	// 抗积分饱和（条件积分）：输出饱和且误差仍在推高饱和方向时冻结积分。
+	// 不用大增益反算回退——深饱和时（P 项单独超限幅）反算会把积分压成反号，
+	// 误差一缩小输出瞬间反向，实测表现为到位疯狂抖动/停不到位。
+	if (output != output_unsat) {
+		if ((output_unsat > limit && error > 0.0f) ||
+			(output_unsat < -limit && error < 0.0f)) {
+			integral = integral_prev; // 冻结：不再向饱和方向累积
+		}
+	}
 	integral = _constrain(integral, -limit, limit);
 
-	// D环（微分环节）
-	float derivative = D * (error - error_prev) / TIME_SLICE;
-
-	// 将P,I,D三环的计算值加起来
-	float output = proportional + integral + derivative;
-	output = _constrain(output, -limit, limit);
-
 	// 输出斜坡限制
-	// 简化的斜坡限制（单行逻辑）
 	if (output_ramp > 0) {
 		float max_delta = output_ramp * TIME_SLICE;
 		output = output_prev +
@@ -52,6 +74,25 @@ float PIDController::operator()(float error) {
 	return output;
 }
 
+/**
+ * @brief 外部饱和泄放积分：前馈叠加后超出限幅时调用
+ * @param[in] excess 超出限幅的部分（带符号）
+ * @return void
+ * @note 仅当积分与超限同号（积分在加剧超限）时泄放，且不泄过零点：
+ *       防止前馈导致的饱和把积分抽干成反号，到位段失去保持力。
+ */
+void PIDController::bleed_integral(float excess) {
+	if (excess > 0.0f && integral_prev > 0.0f) {
+		float bleed = ANTI_WINDUP_GAIN * excess;
+		integral_prev = (integral_prev > bleed) ? (integral_prev - bleed)
+												: 0.0f;
+	} else if (excess < 0.0f && integral_prev < 0.0f) {
+		float bleed = ANTI_WINDUP_GAIN * (-excess);
+		integral_prev = (integral_prev < -bleed) ? (integral_prev + bleed)
+												 : 0.0f;
+	}
+}
+
 /**
  * @brief 设置PID参数
  *
@@ -93,6 +134,9 @@ void PIDController::reset() {
 	error_prev = 0.0f;
 	output_prev = 0.0f;
 	integral_prev = 0.0f;
+	measurement_prev = 0.0f;
+	derivative_filt = 0.0f;
+	deriv_init = false;
 }
 
 /**