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

@@ -1,6 +1,8 @@
 #ifndef TASKS_H
 #define TASKS_H
 
+
+#include "config.h"
 #include "initial.h"
 
 #ifdef __cplusplus
@@ -14,6 +16,19 @@ extern "C" {
  */
 void do_task(u32 currentTasks);
 
+/**
+ * @brief 读取上一统计周期(1s)内指定任务累计消耗的 CPU 周期数
+ * @param[in] task SystemTask 枚举值
+ * @return u32 周期数（120MHz 下 120000 = 1% CPU）
+ */
+u32 get_task_busy_cycles(u8 task);
+
+/**
+ * @brief 清零按任务负载统计（SysTick 每秒统计 cpu_usage 时同步调用）
+ * @return void
+ */
+void task_busy_cycles_reset(void);
+
 #ifdef __cplusplus
 }
 #endif