feat(FOC): 添加角度同步Uq前馈补偿和速度环优化

- 实现update_angle_sync_uq_feedforward函数，支持mode2和mode3的角度同步  Uq前馈补偿，按物理360°查表学习负载/阻力差异
- 实现get_mode3_velocity_p函数，mode3低速时降低速度环P参数减少抖动
- 在velocity_control和angle_control_with_velocity_limit中集成Uq前馈补偿
- 优化normalizeAngle360函数，大角度时先扣整圈数避免while循环过多
- 调整配置参数，降低DEFAULT_vel_loop_p并添加详细的Uq前馈补偿配置
- 更新电源电压参数使用本周期采样值，避免电压滞后影响

Robot_V2/sys/config.h

@@ -126,16 +126,45 @@ constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P
 constexpr float current_loop_Ki = 0.55f;				  // 电流 P
 constexpr float current_loop_Kd = 3e-4f;				  // 电流 P
 
-constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度 P
+constexpr float DEFAULT_vel_loop_p = 0.005f / 100 * 12; // 速度 P
 constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
 constexpr float vel_loop_Tu = 17.0f * 1e-3f;
 constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
 constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
 
+// mode2 角度同步 Uq 前馈补偿：固定 Uq 下的真实速度波动不应从反馈中滤掉，
+// 而应按物理 360° 提前补偿负载/阻力差异，降低速度 PID 的周期性纠偏量。
+constexpr bool MODE2_UQ_FF_ENABLE = true;
+constexpr bool MODE2_UQ_FF_LEARN_ENABLE = true;
+constexpr u16 MODE2_UQ_FF_TABLE_SIZE = 180;
+constexpr float MODE2_UQ_FF_LEARN_ALPHA = 0.01f; // 越小越慢，避免误学瞬态
+constexpr float MODE2_UQ_FF_MIN_TARGET = 1.5f;    // 目标速度过低时不学习
+constexpr float MODE2_UQ_FF_GAIN = 0.003f;       // 速度误差(deg/s)到前馈电压(V)的学习比例
+constexpr float MODE2_UQ_FF_MAX = 3.5f;           // 前馈补偿电压限幅(V)
+constexpr float MODE2_UQ_FF_STOP_SPEED = 2.0f;    // 目标/实际速度低于该值时不输出前馈
+constexpr float MODE2_UQ_FF_BRAKE_ERR = 3.0f;     // 明显超速/刹车时不输出前馈，单位同 velocity_error
+
+// mode3 角度同步 Uq 前馈补偿：单独建表，避免和 mode2 互相污染。
+constexpr bool MODE3_UQ_FF_ENABLE = true;
+constexpr bool MODE3_UQ_FF_LEARN_ENABLE = true;
+constexpr u16 MODE3_UQ_FF_TABLE_SIZE = 180;
+constexpr float MODE3_UQ_FF_LEARN_ALPHA = 0.01f;
+constexpr float MODE3_UQ_FF_MIN_TARGET = 2.5f;    // mode3 内部目标速度过低时不学习
+constexpr float MODE3_UQ_FF_GAIN = 0.3f;         // mode3 速度误差未乘减速比，故增益独立于 mode2
+constexpr float MODE3_UQ_FF_MAX = 3.5f;
+constexpr float MODE3_UQ_FF_STOP_SPEED = 2.0f;    // mode3 内部目标/实际速度低于该值时不输出前馈
+constexpr float MODE3_UQ_FF_BRAKE_ERR = 3.0f;     // 明显超速/刹车时不输出前馈，单位同 velocity_error
+
 constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P
 constexpr float vel_angloop_Ki = 5.0f;				// 速度 速度 - 角度 I
 constexpr float vel_angloop_Kd = 5e-05f;			// 速度 速度 - 角度 D
 
+// mode3 低速/静止附近速度 P 降低，减少速度反馈噪声、齿槽和摩擦导致的来回拉扯。
+constexpr bool MODE3_LOW_SPEED_P_SCALE_ENABLE = true;
+constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.2f;
+constexpr float MODE3_LOW_SPEED_P_LOW = 1.0f;
+constexpr float MODE3_LOW_SPEED_P_FULL = 5.0f;
+
 constexpr float DEFAULT_angle_angloop_p =
 	150.0f / 100 * 12;					 // 角度 速度 - 角度 P
 constexpr float ang_angloop_Ki = 0.00f;	 // 角度 速度 - 角度 I

Robot_V2/sys/health.cpp

@@ -115,8 +115,8 @@ void power_manage(void) {
 	sampler.update_vbus_voltage();
 	float vbus = sampler.get_vbus_voltage();
 
-	// 更新FOC的电源电压参数
-	FOC.update_power(last_vbus_slow);
+	// 更新FOC的电源电压参数：使用本周期刚采到的 vbus，避免上一拍电压滞后放大速度/电流耦合。
+	FOC.update_power(vbus);
 	last_vbus_slow = vbus;
 
 	// 如果快速检测已触发紧急关机，执行保存