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

@@ -48,7 +48,6 @@ class FOC_Controller {
 	float voltage_power_supply = 24.0f; // 电源电压
 	float v_safe = voltage_power_supply / 1.05f;
 	float duty_a = 0, duty_b = 0, duty_c = 0;
-	float Ua = 0, Ub = 0, Uc = 0;
 
 	// 快速电压检测相关成员（基于跌幅检测）
 	float vbus_reference = 0.0f;	// 基准电压（峰值检测，跟踪当前额定电压）
@@ -250,15 +249,19 @@ class FOC_Controller {
 	#endif // FOC_DEBUG
 
 	// 此处默认值不作数
+	// 注意：构造期 limit_voltage 尚为 0，分母一律用 voltage_power_supply(24V)，
+	// 否则 P 初始化为 +inf；运行期各模式每周期 set_P/set_PID 覆盖。
+	// 内环 output_ramp 保持原值：实测收紧到 2000 V/s 会在制动段引入相位滞后，
+	// 导致到位抖动，已回退。
 	PIDController vel_loop =
-		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
-					  vel_loop_Kd, 10000, voltage_power_supply}; // 12V
+		PIDController{DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
+					  vel_loop_Kd, 10000, voltage_power_supply};
 	PIDController angle_loop =
-		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
-					  10000000, voltage_power_supply}; // 12V
+		PIDController{DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
+					  0.02f, 10000000, voltage_power_supply};
 	PIDController current_loop = PIDController{
-		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
-		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+		DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply};
 	PIDController vel_inVelAngloop =
 		PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
 					  vel_angloop_Kd, 1e6, v_safe};
@@ -267,13 +270,13 @@ class FOC_Controller {
 					  ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
 
 	PIDController ang_inMITloop =
-		PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
+		PIDController{DEFAULT_angle_MIT_p * 100 / voltage_power_supply, angle_MIT_Ki,
 					  angle_MIT_Kd, 1e5, 100};
 	PIDController vel_inMITloop = PIDController{
 		DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
 	PIDController current_inMITloop = PIDController{
-		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
-		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+		DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply};
 
 	/**
 	 * @brief 更新 pid_limits 相关状态
@@ -289,32 +292,40 @@ class FOC_Controller {
   protected:
 	float shaft_angle = 0;
 	float Ualpha, Ubeta = 0;
-	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	float startup_ramp_val = 0.0f;      // 软启动系数
-	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
+	// 本周期电角度的 sin/cos：update_el_sincos() 每周期算一次，
+	// Park 变换与逆 Park 变换共用，避免同一角度算两次三角函数
+	float el_sin = 0.0f;
+	float el_cos = 0.0f;
+	/**
+	 * @brief 每周期计算一次电角度 sin/cos 供 Park/逆Park 共用
+	 * @param[in] angle_el 电角度（度）
+	 * @return void
+	 */
+	inline void update_el_sincos(float angle_el);
 	/**
 	 * @brief 设置 setSVPWM_PhaseVoltage 对应的参数或状态
 	 * @param[in] Uq 参数
 	 * @param[in] Ud 参数
-	 * @param[in] angle_el 参数
 	 * @return void
 	 */
-	void setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el);
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	/**
 	 * @brief cal_Iq_Id 函数
 	 * @param[in] current_a 参数
 	 * @param[in] current_b 参数
 	 * @param[in] current_c 参数
-	 * @param[in] angle_el 参数
 	 * @return float 返回值
+	 * @note 电气角度三角函数用 update_el_sincos() 的预计算结果
 	 */
-	float cal_Iq_Id(float current_a, float current_b, float current_c, float angle_el);
+	float cal_Iq_Id(float current_a, float current_b, float current_c);
 	float update_angle_sync_uq_feedforward(u8 mode, float table[],
 									 u16 table_size, float mech_angle,
 									 float target_vel, float actual_vel,
 									 float velocity_error);
 	float get_mode3_velocity_p(float target_vel, float actual_vel);
+	float get_mode3_velocity_scale(float target_vel, float actual_vel,
+								   float min_scale);
 };
 
 // 定义函数指针类型