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

@@ -15,6 +15,7 @@
 #include "flash.h"
 #include "health.h"
 #include "initial.h"
+#include "motor.h"
 #include "systick.h"
 
 // MAIN 调试宏定义
@@ -79,8 +80,9 @@ int main(void) {
  */
 u8 hardware_init(void) {
 	__enable_irq();
-	systick_config();  // 系统时钟初始化
-	unused_pin_init(); // 未使用引脚初始化
+	systick_config();   // 系统时钟初始化
+	unused_pin_init();  // 未使用引脚初始化
+	gate_driver_preinit(); // 门驱动使能脚预置安全态（新板：DRVSLP 低=芯片睡眠）
 	USART0_init();	   // 调试串口初始化
 	User_timer_init(); // 任务 Timer 初始化
 
@@ -97,6 +99,11 @@ u8 hardware_init(void) {
 	init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
 	init_status += Vel_kalman.init(0.0f);				  // 速度滤波初始化
 	init_status += FOC.init();							  // FOC 初始化
+
+	// 门驱动上电配置放在最后：此时 TIMER1/PWM 已启动且占空比为 0，
+	// 新板(DRV8323)在此拉高 DRVSLP 唤醒芯片并经 SPI 配置 3x PWM/增益/保护，
+	// 随后即使任务立即输出 0 占空比，桥臂也处于"下管常开"的静止安全态
+	init_status += gate_driver_bringup();
 	return init_status;
 }