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/sys/flash.h

@@ -7,6 +7,8 @@
 #ifndef __FLASH_H
 #define __FLASH_H
 
+
+#include "config.h"
 #include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
 #include <cstdint>
 
@@ -56,7 +58,6 @@ class FlashEEPROM {
 	static constexpr u8 ADDR_NODE_ID = 0x06;	  // 节点 ID 存储地址
 	static constexpr u8 ADDR_CALIBRATION_ANGLE = 0x07; // 电角度零位校准角存储地址
 
-	bool angle_saved = false;
 	bool system_config_saved = false;
 	u8 EEPROMStatus;
 	// 构造函数
@@ -120,11 +121,6 @@ class FlashEEPROM {
 	 * @return u8 返回值
 	 */
 	u8 read_saved_system_config(void);
-	/**
-	 * @brief 读取保存的角度值
-	 * @return float 返回值
-	 */
-	float read_angle();
 	/**
 	 * @brief 保存电角度零位校准角到 Flash
 	 * @param[in] angle 校准角（度）
@@ -132,22 +128,22 @@ class FlashEEPROM {
 	 */
 	FlashEepromStatus save_calibration_angle(float angle);
 
-	// 节点 ID 独立读写函数（使用魔数+双重备份+计数器机制）
-	// 从 Flash 读取节点 ID（带魔数验证和双重备份回退）
+	// 节点 ID 独立读写函数（主/备双备份分属独立 Flash 页 + 魔数 + 计数器 + CRC）
+	// 从 Flash 读取节点 ID（双备份均有效取计数器最新，单份有效则回退）
 	/**
 	 * @brief 读取节点 ID 配置
 	 * @param[in] node_id 参数
 	 * @return FlashEepromStatus 返回值
 	 */
 	FlashEepromStatus readNodeId(u16 *node_id) const;
-	// 保存节点 ID 到 Flash（写入主备份和备用备份）
+	// 保存节点 ID 到 Flash（乒乓写入非最新页，另一页保有旧值，掉电安全）
 	/**
 	 * @brief 写入节点 ID 配置
 	 * @param[in] node_id 参数
 	 * @return FlashEepromStatus 返回值
 	 */
 	FlashEepromStatus writeNodeId(u16 node_id);
-	// 擦除节点 ID 存储区（恢复默认值）
+	// 擦除节点 ID 双备份存储页（恢复默认值）
 	/**
 	 * @brief 擦除节点 ID 配置
 	 * @return FlashEepromStatus 返回值