feat(FOC): 添加齿槽转矩补偿功能

- 实现齿槽转矩补偿算法，通过查表+线性插值方式计算补偿电压值
- 添加齿槽转矩校准功能，支持正转3次+反转3次的非阻塞校准状态机
- 在所有控制模式中集成齿槽转矩补偿，包括开环、角度、速度、电流和MIT模式
- 硬编码预校准的正反转补偿表数据
- 通过CAN命令支持运行时触发校准功能
- 添加相关配置参数和调试开关

Robot_V2/Algorithm/FOC.h

@@ -70,7 +70,38 @@ class FOC_Controller {
 	void shut_down_thermal(void);	// 过热关机
 	void resume_from_thermal(void); // 过热恢复
 	void fast_voltage_check(void);	// 快速电压检测（100us周期调用）
-	
+
+	// 齿槽转矩补偿
+	bool cogging_comp_enabled = false;
+	float cogging_comp_scale = COGGING_COMP_SCALE;
+	float cogging_table[COGGING_TABLE_SIZE] = {0}; // 齿槽补偿表-正转 (mA)
+	float cogging_table_rev[COGGING_TABLE_SIZE] = {0}; // 齿槽补偿表-反转 (mA)
+	inline float get_cogging_compensation(float angle_el);
+
+	// === 校准相关（DEBUG，平时不启用）===
+	// 启用方法：config.h 中 FOC_DEBUG 改为 1
+	#if FOC_DEBUG
+	volatile bool cogging_calib_request = false;
+	void calibrate_cogging_tick(void);
+	enum class CogCalibState : u8 {
+		IDLE = 0,
+		SETTLING,
+		SAMPLING,
+		STOPPING,
+		DONE
+	};
+	CogCalibState cog_calib_state = CogCalibState::IDLE;
+	u8 cog_calib_run = 0;
+	u32 cog_calib_sample_count = 0;
+	u32 cog_calib_settle_tick = 0;
+	u32 cog_calib_stop_tick = 0;
+	u8 cog_calib_saved_mode = 0;
+	float cog_calib_saved_vel = 0;
+	static float cog_calib_run_data[6][COGGING_TABLE_SIZE]; // 3正+3反
+	static float cog_calib_accum[COGGING_TABLE_SIZE];
+	static u32 cog_calib_count[COGGING_TABLE_SIZE];
+	#endif // FOC_DEBUG
+
 	// 此处默认值不作数
 	PIDController vel_loop =
 		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,