refactor(FOC): 移除齿槽转矩补偿功能

对硬编码的两张 128 点补偿表做谐波分解后确认该补偿无效：
- 能量集中在 k=1(11次/转) 16.1% 与 k=2(22次/转) 82.2%，合计 98.3%；
  而 22 极电机的真实齿槽基频应落在槽极 LCM 处（12槽=132次/转 k=12、
  18槽 k=18、24槽 k=24），实测幅值仅 0.46/0.12/0.08 mA，在噪声地板内。
  即表中属于齿槽的成分不足总能量 0.1%，k=1/k=2 实为电角度零位偏差与
  电流采样通道增益/零点失配的特征。
- 表按电角度查表，而电角度含每台独立的 calibration_angle（Flash 读回）。
  相位偏 φ 时残差比为 2sin(kφ/2)：零点差 30° 电角度时 k=2 残差即等于
  原幅值，补偿由减小扰动变为增大扰动。22 极下 2.7° 机械偏差即达此量级，
  硬编码全局共用一张表不成立。
- 表采集于 bec7c11(2026-05)，其电流链路为改版前方案，此后陆续修正了
  采样时刻落在上管导通窗、PA3/4/5 未配 AIN、零点配平未应用等问题。
  用当时的链路去测量比其自身误差小三个数量级的量，采到的是链路误差。

删除内容：get_cogging_compensation 查表插值、FOC_DEBUG 校准状态机及
其打印函数与静态缓冲、init() 中两张硬编码表、COGGING_TABLE_SIZE/
COGGING_COMP_SCALE、tasks.cpp 调度 tick、RESET 帧 bit4 触发（该位保留）。

六个控制模式的 Uq 输出去掉补偿项；angle_el_cog 仍供 update_el_sincos
使用，改名回 angle_el。mode3 注释中"反转阻力差异由 REV 齿槽补偿表承担"
的描述一并订正（该依赖本已失效）。

Robot_V2/Algorithm/FOC.cpp

@@ -32,233 +32,6 @@ extern EncoderVelocityKalman Vel_kalman; // 速度滤波
 extern CurrentKalman Current_kalman;
 extern FlashEEPROM flashEEP;
 
-// 齿槽转矩补偿：查表 + 线性插值
-// angle_el: 电角度（度）
-// 返回: 补偿电压值 (V)，叠加到 Uq 上
-/**
- * @brief 获取 get_cogging_compensation 对应的数据或状态
- * @param[in] angle_el 参数
- * @return float 返回值
- */
-inline float FOC_Controller::get_cogging_compensation(float angle_el) {
-	if (!cogging_comp_enabled) return 0.0f;
-
-	// angle_el 已由调用方归一化到 [0, 360)，无需重复
-	float angle = angle_el;
-
-	// 线性插值查表
-	float index_f = angle / 360.0f * (float)COGGING_TABLE_SIZE;
-	u16 idx0 = (u16)index_f;
-	u16 idx1 = (idx0 + 1) % COGGING_TABLE_SIZE;
-	float frac = index_f - (float)idx0;
-
-	// 根据转向选择补偿表（带滞回，避免零速附近跳变）
-	static float *last_table = cogging_table;
-	if (rt_velocity < -0.5f) last_table = cogging_table_rev;
-	else if (rt_velocity > 0.5f) last_table = cogging_table;
-	float *table = last_table;
-	float comp = table[idx0] * (1.0f - frac) + table[idx1] * frac;
-
-	return -comp * cogging_comp_scale; // mA→V: 补偿电流(mA) × 换算系数(V/mA)
-}
-
-// === 齿槽转矩校准（DEBUG，平时不启用）===
-// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4）
-// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0
-#if FOC_DEBUG
-// 齿槽转矩校准参数
-constexpr u32 COG_CALIB_SETTLE_TICKS = 16000;       // 800ms / 50us
-constexpr u32 COG_CALIB_STOP_TICKS = 10000;         // 500ms / 50us
-constexpr u32 COG_CALIB_TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
-constexpr float COG_CALIB_SLOW_VEL = 1.0f;          // 开环速度 rad/s
-constexpr u32 COG_CALIB_SAMPLE_DIV = 20;            // 1ms / 50us
-constexpr u8 COG_CALIB_TOTAL_RUNS = 6;              // 3正+3反
-
-// 齿槽转矩校准静态数据
-float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {};
-float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {};
-u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};
-
-/**
- * @brief 齿槽转矩校准 - 非阻塞状态机
- * 正转3次 + 反转3次，分开存储补偿表。
- */
-void FOC_Controller::calibrate_cogging_tick(void) {
-	// 入口只做状态派发，避免单函数堆叠所有校准分支。
-	switch (cog_calib_state) {
-	case CogCalibState::IDLE:
-		start_cogging_calibration();
-		return;
-	case CogCalibState::SETTLING:
-		cogging_settling_tick();
-		return;
-	case CogCalibState::SAMPLING:
-		cogging_sampling_tick();
-		return;
-	case CogCalibState::STOPPING:
-		cogging_stopping_tick();
-		return;
-	case CogCalibState::DONE:
-		cogging_done_tick();
-		return;
-	default:
-		// 防御性处理：枚举值异常时回到空闲态，避免状态机卡死。
-		cog_calib_state = CogCalibState::IDLE;
-		return;
-	}
-}
-
-void FOC_Controller::start_cogging_calibration(void) {
-	// IDLE 状态下没有请求则保持空闲，不扰动正常控制路径。
-	if (!cogging_calib_request) return;
-	cogging_calib_request = false;
-
-	cog_calib_run = 0;
-	// 保存原控制状态，校准完成后恢复现场。
-	cog_calib_saved_mode = control_mode;
-	cog_calib_saved_vel = target_velocity;
-	cogging_comp_enabled = false;
-	memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
-
-	printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
-
-	control_mode = 6;
-	target_velocity = COG_CALIB_SLOW_VEL;
-	cog_calib_settle_tick = 0;
-	cog_calib_state = CogCalibState::SETTLING;
-}
-
-void FOC_Controller::cogging_settling_tick(void) {
-	// 给低速开环运行留出稳定时间，再开始采样。
-	cog_calib_settle_tick++;
-	if (cog_calib_settle_tick < COG_CALIB_SETTLE_TICKS) return;
-
-	cog_calib_state = CogCalibState::SAMPLING;
-	cog_calib_sample_count = 0;
-	memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
-	memset(cog_calib_count, 0, sizeof(cog_calib_count));
-}
-
-void FOC_Controller::cogging_sampling_tick(void) {
-	static u32 sample_div = 0;
-	// 每 1ms 采一次，降低校准数据相关处理对实时控制的影响。
-	sample_div++;
-	if (sample_div < COG_CALIB_SAMPLE_DIV) return;
-	sample_div = 0;
-
-	float real_angle = KTH7823.getcurrent_sensorangle();
-	float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
-	float iq = rt_current;
-
-	if (fabsf(iq) < 5000.0f) {
-		u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
-		if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
-		cog_calib_accum[idx] += iq;
-		cog_calib_count[idx]++;
-	}
-	cog_calib_sample_count++;
-
-	if (cog_calib_sample_count >= COG_CALIB_TARGET_SAMPLES) {
-		target_velocity = 0;
-		cog_calib_state = CogCalibState::STOPPING;
-		cog_calib_stop_tick = 0;
-	}
-}
-
-void FOC_Controller::cogging_stopping_tick(void) {
-	// 每轮采样结束后先停稳，再计算本轮去直流数据。
-	cog_calib_stop_tick++;
-	if (cog_calib_stop_tick < COG_CALIB_STOP_TICKS) return;
-
-	float dc = process_cogging_run_data();
-	printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
-		   cog_calib_run + 1, COG_CALIB_TOTAL_RUNS, dc);
-
-	cog_calib_run++;
-	if (cog_calib_run < COG_CALIB_TOTAL_RUNS) {
-		start_next_cogging_run();
-	} else {
-		cog_calib_state = CogCalibState::DONE;
-	}
-}
-
-float FOC_Controller::process_cogging_run_data(void) {
-	// 先求每个角度桶的平均电流，再统一去除直流偏置。
-	float dc = 0;
-	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
-		cog_calib_run_data[cog_calib_run][i] =
-			(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
-		dc += cog_calib_run_data[cog_calib_run][i];
-	}
-	dc /= (float)COGGING_TABLE_SIZE;
-	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
-		cog_calib_run_data[cog_calib_run][i] -= dc;
-	}
-	return dc;
-}
-
-void FOC_Controller::start_next_cogging_run(void) {
-	control_mode = 6;
-	target_velocity = (cog_calib_run < 3) ? COG_CALIB_SLOW_VEL : -COG_CALIB_SLOW_VEL;
-	cog_calib_state = CogCalibState::SETTLING;
-	cog_calib_settle_tick = 0;
-}
-
-void FOC_Controller::cogging_done_tick(void) {
-	// 生成最终表后恢复用户进入校准前的控制模式和目标速度。
-	build_cogging_tables();
-	cogging_comp_enabled = true;
-	control_mode = cog_calib_saved_mode;
-	target_velocity = cog_calib_saved_vel;
-
-	print_cogging_table("\r\n[COG] === FWD Average ===\r\n", cogging_table);
-	print_cogging_table("\r\n[COG] === REV Average ===\r\n", cogging_table_rev);
-	print_cogging_c_array("\r\n[COG] --- FWD C Array ---\r\n",
-						 "COGGING_TABLE_FWD", cogging_table);
-	print_cogging_c_array("\r\n[COG] --- REV C Array ---\r\n",
-						 "COGGING_TABLE_REV", cogging_table_rev);
-
-	printf("\r\n[COG] === Done. ===\r\n");
-	cog_calib_state = CogCalibState::IDLE;
-}
-
-void FOC_Controller::build_cogging_tables(void) {
-	// 前 3 轮为正转，后 3 轮为反转，分别求平均表。
-	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
-		cogging_table[i] = (cog_calib_run_data[0][i] +
-							 cog_calib_run_data[1][i] +
-							 cog_calib_run_data[2][i]) / 3.0f;
-		cogging_table_rev[i] = (cog_calib_run_data[3][i] +
-								 cog_calib_run_data[4][i] +
-								 cog_calib_run_data[5][i]) / 3.0f;
-	}
-}
-
-void FOC_Controller::print_cogging_table(const char *title, float *table) {
-	printf("%s", title);
-	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
-		float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
-		printf("[COG] %.2f, %.2f\r\n", angle_deg, table[i]);
-		fwdgt_counter_reload();
-	}
-}
-
-void FOC_Controller::print_cogging_c_array(const char *title,
-										   const char *arrayName,
-										   float *table) {
-	printf("%s", title);
-	printf("static const float %s[COGGING_TABLE_SIZE] = {\r\n", arrayName);
-	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
-		if (i % 8 == 0) printf("    ");
-		printf("%.2ff", table[i]);
-		if (i < COGGING_TABLE_SIZE - 1) printf(", ");
-		if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
-		fwdgt_counter_reload();
-	}
-	printf("};\r\n");
-}
-#endif // FOC_DEBUG
-
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
@@ -455,7 +228,7 @@ float FOC_Controller::get_mode3_velocity_scale(float target_vel,
  *       后改为连续线性过渡）：连续化后反转仍剧烈震荡，说明问题不在
  *       台阶突跳，而在增益本身——×2 只对反转生效，把反转环路增益整体
  *       翻倍而正转不变，正是"正转正常、反转震荡"的直接来源之一。
- *       已移除。反转阻力差异由 REV 齿槽补偿表与 Uq 前馈学习承担；
+ *       已移除。反转阻力差异由 Uq 前馈学习承担；
  *       若实测反转欠速/偏慢，优先增大前馈学习限幅，不要恢复非对称 P。
  */
 float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
@@ -471,47 +244,6 @@ float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
 u8 FOC_Controller::init() {
 	motor_init(); // 电机驱动初始化
 
-	// 齿槽转矩补偿表（硬编码，3正+3反平均，用真实角度算Iq）
-	static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {
-		29.28f, 28.84f, 28.26f, 28.83f, 29.79f, 30.26f, 29.52f, 28.77f,
-		28.23f, 27.68f, 26.65f, 25.19f, 23.92f, 22.59f, 19.61f, 16.77f,
-		13.32f, 10.40f, 7.64f, 3.51f, -0.83f, -4.96f, -8.19f, -11.11f,
-		-13.40f, -14.39f, -15.69f, -15.99f, -16.39f, -15.50f, -14.76f, -14.21f,
-		-13.76f, -13.88f, -13.22f, -11.18f, -10.65f, -10.54f, -10.62f, -10.23f,
-		-9.52f, -8.19f, -6.31f, -4.14f, -1.94f, -0.25f, 2.06f, 4.08f,
-		6.05f, 8.30f, 9.74f, 11.38f, 13.32f, 15.65f, 17.55f, 20.22f,
-		21.72f, 22.28f, 23.30f, 24.78f, 23.53f, 23.35f, 23.00f, 21.42f,
-		20.35f, 20.57f, 20.07f, 18.18f, 17.04f, 16.03f, 14.81f, 13.71f,
-		12.47f, 10.93f, 9.38f, 6.53f, 4.63f, 3.37f, 1.29f, -1.28f,
-		-5.08f, -11.11f, -16.35f, -21.00f, -24.61f, -28.30f, -31.11f, -33.48f,
-		-35.25f, -36.54f, -38.01f, -38.69f, -39.27f, -39.52f, -38.18f, -36.55f,
-		-34.96f, -33.14f, -31.92f, -31.45f, -31.32f, -30.25f, -29.60f, -28.72f,
-		-27.51f, -25.30f, -22.66f, -20.87f, -18.19f, -16.10f, -13.25f, -10.81f,
-		-8.00f, -4.67f, -2.28f, 0.87f, 3.83f, 7.25f, 10.56f, 13.70f,
-		17.70f, 19.90f, 22.14f, 23.62f, 24.53f, 26.03f, 26.96f, 27.95f,
-	};
-	static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {
-		22.45f, 24.95f, 26.05f, 26.01f, 26.11f, 25.87f, 26.20f, 26.23f,
-		26.19f, 25.87f, 25.97f, 26.09f, 24.77f, 24.22f, 23.71f, 22.10f,
-		19.84f, 17.60f, 15.36f, 12.99f, 9.54f, 5.35f, 1.71f, -1.94f,
-		-4.15f, -6.16f, -7.93f, -9.10f, -10.01f, -10.25f, -10.85f, -11.74f,
-		-11.98f, -10.78f, -9.65f, -8.89f, -8.14f, -6.85f, -5.94f, -5.37f,
-		-4.28f, -3.38f, -3.36f, -3.40f, -2.81f, -2.36f, -1.04f, -0.18f,
-		1.08f, 2.15f, 4.19f, 5.51f, 7.33f, 8.64f, 10.46f, 11.89f,
-		13.42f, 14.83f, 17.11f, 19.12f, 20.76f, 21.88f, 22.35f, 22.86f,
-		22.73f, 22.13f, 21.10f, 20.53f, 18.65f, 17.09f, 15.47f, 14.76f,
-		14.02f, 12.94f, 11.98f, 10.29f, 8.11f, 5.83f, 4.23f, 2.42f,
-		1.03f, -1.80f, -4.01f, -7.93f, -11.79f, -15.90f, -20.22f, -23.04f,
-		-25.66f, -28.80f, -30.45f, -32.11f, -33.17f, -33.97f, -34.68f, -34.26f,
-		-34.18f, -33.78f, -32.47f, -31.11f, -29.40f, -27.89f, -27.05f, -26.72f,
-		-25.59f, -24.86f, -23.61f, -22.97f, -22.89f, -21.81f, -20.78f, -19.23f,
-		-17.74f, -15.09f, -13.18f, -10.51f, -8.18f, -5.89f, -3.35f, -0.02f,
-		2.02f, 3.87f, 6.37f, 9.11f, 12.33f, 14.63f, 17.12f, 19.12f,
-	};
-	memcpy(cogging_table, COGGING_TABLE_FWD, sizeof(COGGING_TABLE_FWD));
-	memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
-	cogging_comp_enabled = true;
-
 	// limit_voltage 从 Flash/CAN 恢复，可能为 0：分母下限保护，防止 P=inf
 	const float denom = (limit_voltage > MIN_LIMIT_VOLTAGE)
 							? limit_voltage
@@ -820,9 +552,9 @@ float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
 	shaft_angle =
 		normalizeAngle360(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY);
-	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
-	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
-	update_el_sincos(angle_el_cog);
+	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
+	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
 						   sampler.get_current('W'));
 
@@ -842,7 +574,7 @@ float FOC_Controller::velocity_open_loop() {
 		}
 		Uq = (Uq < 0.0f) ? -Uq_mag : Uq_mag;
 	}
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return limit_voltage;
 }
 
@@ -859,13 +591,13 @@ float FOC_Controller::angle_control() {
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
-	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
-	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
-	update_el_sincos(angle_el_cog);
+	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
+	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
 						   sampler.get_current('W'));
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
 }
 
@@ -917,11 +649,11 @@ float FOC_Controller::velocity_control() {
 	}
 	CAN0_transmiter.testfloat_union.f = Uq;
 
-	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
-	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
-	update_el_sincos(angle_el_cog);
+	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
+	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el);
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
 						   sampler.get_current('W'));
 
@@ -936,9 +668,9 @@ float FOC_Controller::current_control() {
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	CAN0_transmiter.testfloat_union.f = shaft_angle;
-	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
-	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
-	update_el_sincos(angle_el_cog);
+	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
+	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el);
 	// PID 控制计算
 	float Iq = rt_current =
 		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
@@ -951,7 +683,7 @@ float FOC_Controller::current_control() {
 		Uq = -limit_voltage;
 	}
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return 1;
 }
 
@@ -1008,14 +740,14 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	}
 	CAN0_transmiter.testfloat_union.f = shaft_angle;
 
-	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
-	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
-	update_el_sincos(angle_el_cog);
+	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
+	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el);
 	// 获取当前实际电流值
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
 						   sampler.get_current('W'));
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 
 	return shaft_angle;
 }
@@ -1050,9 +782,9 @@ float FOC_Controller::MIT_Mode() {
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
-	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
-	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
-	update_el_sincos(angle_el_cog);
+	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
+	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el);
 
 	// 第三层：电流环（内环）
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
@@ -1063,7 +795,7 @@ float FOC_Controller::MIT_Mode() {
 	float Uq = current_inMITloop(current_error, rt_current);
 
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 
 	return shaft_angle;
 }

Robot_V2/Algorithm/FOC.h

@@ -149,105 +149,6 @@ class FOC_Controller {
 	 */
 	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)
-	/**
-	 * @brief 获取 get_cogging_compensation 对应的数据或状态
-	 * @param[in] angle_el 参数
-	 * @return float 返回值
-	 */
-	inline float get_cogging_compensation(float angle_el);
-
-	// === 校准相关（DEBUG，平时不启用）===
-	// 启用方法：config.h 中 FOC_DEBUG 改为 1
-	#if FOC_DEBUG
-	volatile bool cogging_calib_request = false;
-	/**
-	 * @brief 齿槽校准状态机入口，每个周期按当前状态派发到对应处理函数
-	 * @return void
-	 */
-	void calibrate_cogging_tick(void);
-	/**
-	 * @brief 处理齿槽校准空闲态，收到请求后保存现场并进入稳定等待态
-	 * @return void
-	 */
-	void start_cogging_calibration(void);
-	/**
-	 * @brief 处理齿槽校准稳定等待态，等待电机进入稳定低速运行
-	 * @return void
-	 */
-	void cogging_settling_tick(void);
-	/**
-	 * @brief 处理齿槽校准采样态，按分频周期累计每个电角度桶的电流
-	 * @return void
-	 */
-	void cogging_sampling_tick(void);
-	/**
-	 * @brief 处理齿槽校准停止态，等待停稳后完成本轮数据归一化
-	 * @return void
-	 */
-	void cogging_stopping_tick(void);
-	/**
-	 * @brief 处理齿槽校准完成态，生成补偿表并打印校准结果
-	 * @return void
-	 */
-	void cogging_done_tick(void);
-	/**
-	 * @brief 计算当前校准轮次的平均电流并去除直流偏置
-	 * @return float 当前轮次直流偏置，单位 mA
-	 */
-	float process_cogging_run_data(void);
-	/**
-	 * @brief 切换到下一轮齿槽校准运行方向并重新进入稳定等待态
-	 * @return void
-	 */
-	void start_next_cogging_run(void);
-	/**
-	 * @brief 根据 3 次正转和 3 次反转采样结果生成最终补偿表
-	 * @return void
-	 */
-	void build_cogging_tables(void);
-	/**
-	 * @brief 打印齿槽补偿表的人类可读角度/电流列表
-	 * @param[in] title 表格标题字符串
-	 * @param[in] table 待打印的补偿表
-	 * @return void
-	 */
-	void print_cogging_table(const char *title, float *table);
-	/**
-	 * @brief 打印可直接复制回源码的齿槽补偿 C 数组
-	 * @param[in] title 输出标题字符串
-	 * @param[in] arrayName C 数组名
-	 * @param[in] table 待打印的补偿表
-	 * @return void
-	 */
-	void print_cogging_c_array(const char *title, const char *arrayName,
-							   float *table);
-	/**
-	 * @brief CogCalibState 枚举定义
-	 */
-	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
-
 	// 此处默认值不作数
 	// 注意：构造期 limit_voltage 尚为 0，分母一律用 voltage_power_supply(24V)，
 	// 否则 P 初始化为 +inf；运行期各模式每周期 set_P/set_PID 覆盖。