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/Algorithm/FOC.cpp

@@ -308,7 +308,7 @@ void calibrate_zero_position(void) {
  * @return float 返回值
  */
 inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
-							   float current_c, float angle_el) {
+								   float current_c) {
 	if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
 		current_c == ERROR_ADC_VAL) {
 		return Current_kalman.getCurrent();
@@ -320,19 +320,17 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	current_b -= current_avg;
 	current_c -= current_avg;
 
-	// 电气角度由调用方传入，避免重复计算
 	// Clarke变换 (使用三相电流)
 	float I_alpha, I_beta;
 	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
 			  (1.0f / 3.0f) * current_c;
 	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
 
-	// Park变换
-	float sin_el, cos_el;
-	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
+	// Park变换：sin/cos 由 update_el_sincos() 每周期算一次，
+	// 与 setSVPWM_PhaseVoltage 的逆 Park 共用，省一次三角函数
 	float I_q, I_d;
-	I_q = I_beta * cos_el - I_alpha * sin_el;
-	I_d = I_beta * sin_el + I_alpha * cos_el;
+	I_q = I_beta * el_cos - I_alpha * el_sin;
+	I_d = I_beta * el_sin + I_alpha * el_cos;
 
 	// 返回Iq值，乘以1000以转换为mA
 	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
@@ -354,6 +352,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 	float min_target = 0.0f;
 	float stop_speed = 0.0f;
 	float brake_err = 0.0f;
+	float learn_err_max = 0.0f;
 	float learn_alpha = 0.0f;
 	float gain = 0.0f;
 	float max_ff = 0.0f;
@@ -366,6 +365,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 		min_target = MODE2_UQ_FF_MIN_TARGET;
 		stop_speed = MODE2_UQ_FF_STOP_SPEED;
 		brake_err = MODE2_UQ_FF_BRAKE_ERR;
+		learn_err_max = MODE2_UQ_FF_LEARN_ERR_MAX;
 		learn_alpha = MODE2_UQ_FF_LEARN_ALPHA;
 		gain = MODE2_UQ_FF_GAIN;
 		max_ff = MODE2_UQ_FF_MAX;
@@ -377,6 +377,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 		min_target = MODE3_UQ_FF_MIN_TARGET;
 		stop_speed = MODE3_UQ_FF_STOP_SPEED;
 		brake_err = MODE3_UQ_FF_BRAKE_ERR;
+		learn_err_max = MODE3_UQ_FF_LEARN_ERR_MAX;
 		learn_alpha = MODE3_UQ_FF_LEARN_ALPHA;
 		gain = MODE3_UQ_FF_GAIN;
 		max_ff = MODE3_UQ_FF_MAX;
@@ -386,10 +387,16 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 		return 0.0f;
 	}
 
+	// 超速/制动判定必须沿"运动方向"：velocity_error×dir_sign 是 PID 系误差，
+	// 再乘运动方向符号后，< -brake_err 才表示"沿运动方向明显超速"。
+	// 原实现只乘 dir_sign：反转(负速度)时判定极性恰好相反——反转超速时
+	// 前馈继续输出、反转欠速时前馈反而被切断，已学到的电压(可达±MAX)
+	// 随误差符号通断，形成正转没有、反转独有的 bang-bang 震荡。
+	const float motion_sign = (target_vel * dir_sign >= 0.0f) ? 1.0f : -1.0f;
 	const bool active = (fabsf(target_vel) > stop_speed) &&
 					(fabsf(actual_vel) > stop_speed) &&
 					((target_vel * actual_vel) > 0.0f) &&
-					((velocity_error * dir_sign) > -brake_err);
+					((velocity_error * dir_sign * motion_sign) > -brake_err);
 
 	float angle = normalizeAngle360(mech_angle);
 	float index_f = angle / 360.0f * (float)table_size;
@@ -402,7 +409,11 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 	if (ff > max_ff) ff = max_ff;
 	else if (ff < -max_ff) ff = -max_ff;
 
-	if (learn_enable && active && fabsf(target_vel) > min_target) {
+	// 学习仅在误差带内进行：启停/换向瞬态误差可达上百 °/s，直接学习会把
+	// ±MAX 钳位值写进途经的角度桶，下一轮运动经过该角度时突加满幅前馈
+	// 电压，本身就是一次转矩扰动。
+	if (learn_enable && active && fabsf(target_vel) > min_target &&
+		fabsf(velocity_error * dir_sign) < learn_err_max) {
 		float ff_target = velocity_error * dir_sign * gain;
 		if (ff_target > max_ff) ff_target = max_ff;
 		else if (ff_target < -max_ff) ff_target = -max_ff;
@@ -412,32 +423,44 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 	return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f;
 }
 /**
- * @brief mode3 低速/静止附近降低速度内环 P，运行速度升高后平滑恢复。
+ * @brief mode3 低速缩放因子：静止附近降到 min_scale，速度升高后线性恢复到 1。
+ * @note P/I 使用各自的地板（I 地板更低）：低速段保持"比例主导"，
+ *       积分充电足够慢，压制静摩擦 hunting（停下瞬间小幅来回抖动）。
+ *       D 不衰减，保持阻尼。
  */
-float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
-	float vel_p = vel_angloop_p;
-	if (target_vel < -120.0f) {
-		vel_p = vel_angloop_p * 2.0f;
-	} else if (target_vel < -30.0f) {
-		vel_p = vel_angloop_p * 1.5f;
+float FOC_Controller::get_mode3_velocity_scale(float target_vel,
+											   float actual_vel,
+											   float min_scale) {
+	if (!MODE3_LOW_SPEED_P_SCALE_ENABLE) {
+		return 1.0f;
 	}
 
-	if (MODE3_LOW_SPEED_P_SCALE_ENABLE) {
-		float abs_target = fabsf(target_vel);
-		float abs_speed = fabsf(actual_vel);
-		float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
-		float p_scale = 1.0f;
-		if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
-			p_scale = MODE3_LOW_SPEED_P_MIN_SCALE;
-		} else if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
-			p_scale = MODE3_LOW_SPEED_P_MIN_SCALE +
-					  (1.0f - MODE3_LOW_SPEED_P_MIN_SCALE) *
-					  (speed_ref - MODE3_LOW_SPEED_P_LOW) /
-					  (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
-		}
-		vel_p *= p_scale;
+	float abs_target = fabsf(target_vel);
+	float abs_speed = fabsf(actual_vel);
+	float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
+	if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
+		return min_scale;
 	}
-	return vel_p;
+	if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
+		return min_scale +
+			   (1.0f - min_scale) * (speed_ref - MODE3_LOW_SPEED_P_LOW) /
+				   (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
+	}
+	return 1.0f;
+}
+
+/**
+ * @brief mode3 速度内环 P：低速缩放因子（方向无关）。
+ * @note 历史上曾有"反转 P 提升到 ×2"的增益调度（先是 −120/−30 硬台阶，
+ *       后改为连续线性过渡）：连续化后反转仍剧烈震荡，说明问题不在
+ *       台阶突跳，而在增益本身——×2 只对反转生效，把反转环路增益整体
+ *       翻倍而正转不变，正是"正转正常、反转震荡"的直接来源之一。
+ *       已移除。反转阻力差异由 REV 齿槽补偿表与 Uq 前馈学习承担；
+ *       若实测反转欠速/偏慢，优先增大前馈学习限幅，不要恢复非对称 P。
+ */
+float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
+	return vel_angloop_p * get_mode3_velocity_scale(target_vel, actual_vel,
+													MODE3_LOW_SPEED_P_MIN_SCALE);
 }
 
 /**
@@ -489,14 +512,23 @@ u8 FOC_Controller::init() {
 	memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
 	cogging_comp_enabled = true;
 
-	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
-	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
-	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
+	// limit_voltage 从 Flash/CAN 恢复，可能为 0：分母下限保护，防止 P=inf
+	const float denom = (limit_voltage > MIN_LIMIT_VOLTAGE)
+							? limit_voltage
+							: voltage_power_supply;
+	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / denom;
+	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / denom;
+	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / denom;
 	FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
 	FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
-	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
-	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
-	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
+	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / denom;
+	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / denom;
+	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / denom;
+
+	// 运行期不每周期 set_P 的环路（mode1/mode5）在此同步一次有效增益，
+	// 避免沿用构造期的临时值
+	angle_loop.set_P(ang_loop_p);
+	current_loop.set_P(cur_loop_p);
 	return 0;
 }
 
@@ -506,6 +538,15 @@ u8 FOC_Controller::init() {
  */
 FOC_Controller::FOC_Controller() { ; }
 
+/**
+ * @brief 每控制周期调用一次：计算当前电角度的 sin/cos。
+ * @note cal_Iq_Id 的 Park 变换与 setSVPWM_PhaseVoltage 的逆 Park 变换
+ *       使用同一电角度，三角函数每周期只算这一次，两处共用。
+ */
+inline void FOC_Controller::update_el_sincos(float angle_el) {
+	arm_sin_cos_f32(angle_el, &el_sin, &el_cos);
+}
+
 /**
  * @brief 使用SVPWM算法设置电机输出
  *
@@ -514,13 +555,12 @@ FOC_Controller::FOC_Controller() { ; }
  *
  * 优化：atan2f → X/Y/Z 符号法判扇区（~15周期 vs atan2f ~300周期）
  *       边界上扇区编号可能与 atan2f 差1，但 duty 输出完全一致（已数值验证）
+ *       逆 Park 所需 sin/cos 由 update_el_sincos() 预计算，不再重复计算
  */
-inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el) {
-	// ① 逆Park变换（angle_el 由调用方传入）
-	float sin_el, cos_el;
-	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
-	float Ualpha = Ud * cos_el - Uq * sin_el;
-	float Ubeta  = Ud * sin_el + Uq * cos_el;
+inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
+	// ① 逆Park变换（sin/cos 为本周期预计算值）
+	float Ualpha = Ud * el_cos - Uq * el_sin;
+	float Ubeta  = Ud * el_sin + Uq * el_cos;
 
 	// ② 扇区判定（替代 atan2f）
 	float X =  Ubeta;
@@ -727,11 +767,11 @@ void FOC_Controller::update_power(float voltage) {
  * @return void
  */
 void FOC_Controller::fast_voltage_check(void) {
-	// ADC1 单次转换读取 VBUS（ch6，11:1 分压）。
-	// 旧实现读 regular DMA 的 adc_value[2]，通道顺序调整后该下标已是 DRV温度，
-	// 属漏改：NTC 分压值(~1.06V)按 11 倍分压换算仅 ~11.5V，逼近跌落阈值。
+	// ADC1 连续模式零等待读最新 VBUS（ch6，11:1 分压）。
+	// 旧实现 adc1_read_raw 阻塞式"触发+等EOC"：10kHz 调用每次 ~1.5μs 死等。
+	// 通道顺序问题的历史注释已随 ADC1 连续模式改造失效（本函数只读 DR）。
 	float current_vbus =
-		adc1_read_raw(ADC_CHANNEL_6) * (3.3f / 4096.0f) * 11.0f;
+		adc1_read_vbus_latest() * (3.3f / 4096.0f) * 11.0f;
 
 	// 更新基准电压（峰值检测）
 	// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
@@ -782,8 +822,9 @@ float FOC_Controller::velocity_open_loop() {
 		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);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 
 	// 因为在开环控制中，Ud 始终为 0，只控制 Uq
 	// 幅度限幅必须保留符号：原 _constrain 下限为 +2V，负速度会被钳到 +2V，
@@ -801,7 +842,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, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	return limit_voltage;
 }
 
@@ -812,17 +853,19 @@ float FOC_Controller::velocity_open_loop() {
  * @return float: 当前角度
  */
 float FOC_Controller::angle_control() {
-	// PID 控制计算
-	float Uq = angle_loop(target_angle - rt_angle);
+	// PID 控制计算（D 项对测量微分+低通，目标阶跃无微分踢）
+	angle_loop.set_P(ang_loop_p);
+	float Uq = angle_loop(target_angle - rt_angle, rt_angle);
 
 	// 获取电机轴角度
 	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);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	return shaft_angle;
 }
 
@@ -842,24 +885,45 @@ float FOC_Controller::velocity_control() {
 	// mode2 使用独立速度环参数。
 	vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd);
 	vel_loop.set_limit(limit_voltage);
-	float Uq_pid = vel_loop(velocity_error);
+
+	// 仅"实质性换向"时清积分：目标速度变号且新幅值够大（真实反向指令）才清。
+	// 最终贴合阶段误差过零引起的微小变号（|target|=150×0.01°≈1.5°/s）不清——
+	// 否则积分被反复清空，低速段缩小的 P 项（0.12V@0.01°）推不过静摩擦，
+	// 永远停在差 ~0.01° 的位置
+	static float last_mode2_target_vel = 0.0f;
+	if (last_mode2_target_vel * target_velocity < 0.0f &&
+		fabsf(target_velocity) > 5.0f) {
+		vel_loop.reset_integral();
+	}
+	last_mode2_target_vel = target_velocity;
+
+	float Uq_pid =
+		vel_loop(velocity_error, rt_velocity * DIR * REDUCTION_RATIO);
 
 	static float uq_ff_table[MODE2_UQ_FF_TABLE_SIZE] = {0.0f};
 	float Uq_ff = update_angle_sync_uq_feedforward(
 		2, uq_ff_table, MODE2_UQ_FF_TABLE_SIZE, shaft_angle,
 		target_velocity, rt_velocity, velocity_error);
 
+	// PID 输出已内部钳位；前馈叠加后超限时泄放积分，
+	// 让 PID 感知真实饱和（否则重载下积分继续累积→释载超调振荡）
 	float Uq = Uq_pid + Uq_ff;
-	if (Uq > limit_voltage) Uq = limit_voltage;
-	else if (Uq < -limit_voltage) Uq = -limit_voltage;
+	if (Uq > limit_voltage) {
+		vel_loop.bleed_integral(Uq - limit_voltage);
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		vel_loop.bleed_integral(Uq + limit_voltage);
+		Uq = -limit_voltage;
+	}
 	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);
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 
 	return rt_velocity;
 }
@@ -874,18 +938,20 @@ float FOC_Controller::current_control() {
 	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);
 	// PID 控制计算
 	float Iq = rt_current =
 		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-				  sampler.get_current('W'), angle_el_cog);
-	float Uq = current_loop(target_current - Iq);
+				  sampler.get_current('W'));
+	current_loop.set_P(cur_loop_p);
+	float Uq = current_loop(target_current - Iq, Iq);
 	if (Uq > limit_voltage) {
 		Uq = limit_voltage;
 	} else if (Uq < -limit_voltage) {
 		Uq = -limit_voltage;
 	}
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	return 1;
 }
 
@@ -901,21 +967,28 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	float angle_error = target_angle - rt_angle;
 
 	ang_inVelAngloop.set_P(ang_angloop_p);
-	// 使用角度PID控制器计算目标速度
-	float target_velocity = ang_inVelAngloop(angle_error);
+	// 使用角度PID控制器计算目标速度（D 项对测量微分+低通）
+	float target_velocity = ang_inVelAngloop(angle_error, rt_angle);
 
 	// 使用速度PID控制器计算最终输出电压
-	vel_inVelAngloop.set_P(get_mode3_velocity_p(target_velocity, rt_velocity));
+	// 低速段 P/I 各自按地板衰减（D 保持满值）：I 地板更低（0.15），
+	// 停止瞬间积分充电速率仅为全速的 15%，进一步压制静摩擦 hunting
+	float i_scale = get_mode3_velocity_scale(target_velocity, rt_velocity,
+											 MODE3_LOW_SPEED_I_MIN_SCALE);
+	vel_inVelAngloop.set_PID(get_mode3_velocity_p(target_velocity, rt_velocity),
+							 vel_angloop_Ki * i_scale, vel_angloop_Kd);
 	float velocity_error = target_velocity - rt_velocity;
 
-	// 目标速度突变检测：target 大幅变化时清积分
+	// 仅"实质性换向"时清积分（变号且新幅值>5°/s）。制动段不清（防欠程）；
+	// 最终贴合阶段误差过零的微小变号也不清（防积分反复被清、停在差0.01°）
 	static float last_vang_target_vel = 0.0f;
-	if (fabsf(target_velocity - last_vang_target_vel) > 5.0f) {
+	if (last_vang_target_vel * target_velocity < 0.0f &&
+		fabsf(target_velocity) > 5.0f) {
 		vel_inVelAngloop.reset_integral();
 	}
 	last_vang_target_vel = target_velocity;
 
-	float Uq_pid = vel_inVelAngloop(velocity_error) * DIR;
+	float Uq_pid = vel_inVelAngloop(velocity_error, rt_velocity) * DIR;
 
 	// 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。
 	shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -924,18 +997,25 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 		3, mode3_uq_ff_table, MODE3_UQ_FF_TABLE_SIZE, shaft_angle,
 		target_velocity, rt_velocity, velocity_error);
 
+	// 前馈叠加后超限时泄放积分（×DIR 补偿：Uq = PID输出×DIR + 前馈）
 	float Uq = Uq_pid + Uq_ff;
-	if (Uq > limit_voltage) Uq = limit_voltage;
-	else if (Uq < -limit_voltage) Uq = -limit_voltage;
+	if (Uq > limit_voltage) {
+		vel_inVelAngloop.bleed_integral(DIR * (Uq - limit_voltage));
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		vel_inVelAngloop.bleed_integral(DIR * (Uq + limit_voltage));
+		Uq = -limit_voltage;
+	}
 	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);
 	// 获取当前实际电流值
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 
 	return shaft_angle;
 }
@@ -952,36 +1032,38 @@ float FOC_Controller::MIT_Mode() {
 	float angle_error = target_angle - rt_angle;
 	ang_inMITloop.set_P(ang_MIT_p);
 	float target_velocity =
-		ang_inMITloop(angle_error);
+		ang_inMITloop(angle_error, rt_angle);
 
 	// 第二层：速度环（中环）
 	float velocity_error = (target_velocity - rt_velocity)* DIR;
 
-	// 目标速度变化检测：target 大幅变化时清积分
+	// 仅"实质性换向"时清积分（变号且新幅值>5°/s，理由同 mode3）
 	static float last_mit_target_vel = 0.0f;
-	if (fabsf(target_velocity - last_mit_target_vel) > 5.0f) {
+	if (last_mit_target_vel * target_velocity < 0.0f &&
+		fabsf(target_velocity) > 5.0f) {
 		vel_inMITloop.reset_integral();
 	}
 	last_mit_target_vel = target_velocity;
 
 	vel_inMITloop.set_P(vel_MIT_p);
-	float target_current = vel_inMITloop(velocity_error);
+	float target_current = vel_inMITloop(velocity_error, rt_velocity * DIR);
 
 	// 获取电机轴角度
 	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);
 
 	// 第三层：电流环（内环）
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 
 	float current_error = target_current - rt_current;
 	current_inMITloop.set_P(cur_MIT_p);
-	float Uq = current_inMITloop(current_error);
+	float Uq = current_inMITloop(current_error, rt_current);
 
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 
 	return shaft_angle;
 }

Robot_V2/Algorithm/FOC.h

@@ -48,7 +48,6 @@ class FOC_Controller {
 	float voltage_power_supply = 24.0f; // 电源电压
 	float v_safe = voltage_power_supply / 1.05f;
 	float duty_a = 0, duty_b = 0, duty_c = 0;
-	float Ua = 0, Ub = 0, Uc = 0;
 
 	// 快速电压检测相关成员（基于跌幅检测）
 	float vbus_reference = 0.0f;	// 基准电压（峰值检测，跟踪当前额定电压）
@@ -250,15 +249,19 @@ class FOC_Controller {
 	#endif // FOC_DEBUG
 
 	// 此处默认值不作数
+	// 注意：构造期 limit_voltage 尚为 0，分母一律用 voltage_power_supply(24V)，
+	// 否则 P 初始化为 +inf；运行期各模式每周期 set_P/set_PID 覆盖。
+	// 内环 output_ramp 保持原值：实测收紧到 2000 V/s 会在制动段引入相位滞后，
+	// 导致到位抖动，已回退。
 	PIDController vel_loop =
-		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
-					  vel_loop_Kd, 10000, voltage_power_supply}; // 12V
+		PIDController{DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
+					  vel_loop_Kd, 10000, voltage_power_supply};
 	PIDController angle_loop =
-		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
-					  10000000, voltage_power_supply}; // 12V
+		PIDController{DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
+					  0.02f, 10000000, voltage_power_supply};
 	PIDController current_loop = PIDController{
-		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
-		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+		DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply};
 	PIDController vel_inVelAngloop =
 		PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
 					  vel_angloop_Kd, 1e6, v_safe};
@@ -267,13 +270,13 @@ class FOC_Controller {
 					  ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
 
 	PIDController ang_inMITloop =
-		PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
+		PIDController{DEFAULT_angle_MIT_p * 100 / voltage_power_supply, angle_MIT_Ki,
 					  angle_MIT_Kd, 1e5, 100};
 	PIDController vel_inMITloop = PIDController{
 		DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
 	PIDController current_inMITloop = PIDController{
-		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
-		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+		DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply};
 
 	/**
 	 * @brief 更新 pid_limits 相关状态
@@ -289,32 +292,40 @@ class FOC_Controller {
   protected:
 	float shaft_angle = 0;
 	float Ualpha, Ubeta = 0;
-	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	float startup_ramp_val = 0.0f;      // 软启动系数
-	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
+	// 本周期电角度的 sin/cos：update_el_sincos() 每周期算一次，
+	// Park 变换与逆 Park 变换共用，避免同一角度算两次三角函数
+	float el_sin = 0.0f;
+	float el_cos = 0.0f;
+	/**
+	 * @brief 每周期计算一次电角度 sin/cos 供 Park/逆Park 共用
+	 * @param[in] angle_el 电角度（度）
+	 * @return void
+	 */
+	inline void update_el_sincos(float angle_el);
 	/**
 	 * @brief 设置 setSVPWM_PhaseVoltage 对应的参数或状态
 	 * @param[in] Uq 参数
 	 * @param[in] Ud 参数
-	 * @param[in] angle_el 参数
 	 * @return void
 	 */
-	void setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el);
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	/**
 	 * @brief cal_Iq_Id 函数
 	 * @param[in] current_a 参数
 	 * @param[in] current_b 参数
 	 * @param[in] current_c 参数
-	 * @param[in] angle_el 参数
 	 * @return float 返回值
+	 * @note 电气角度三角函数用 update_el_sincos() 的预计算结果
 	 */
-	float cal_Iq_Id(float current_a, float current_b, float current_c, float angle_el);
+	float cal_Iq_Id(float current_a, float current_b, float current_c);
 	float update_angle_sync_uq_feedforward(u8 mode, float table[],
 									 u16 table_size, float mech_angle,
 									 float target_vel, float actual_vel,
 									 float velocity_error);
 	float get_mode3_velocity_p(float target_vel, float actual_vel);
+	float get_mode3_velocity_scale(float target_vel, float actual_vel,
+								   float min_scale);
 };
 
 // 定义函数指针类型

Robot_V2/Algorithm/Filter.cpp

@@ -58,17 +58,38 @@ CurrentKalman Current_kalman(	CURRENT_KALMAN_INITIAL_STATE, CURRENT_KALMAN_INITI
  \param[in] values 待滤波的数据数组
  \param[in] size 数组大小
  \retval    滤波后的中值
- \note      使用部分排序优化算法提高效率
+ \note      size==6（ADC_FILTER_SIZE，唯一实际调用）走固定 12 次比较的
+            排序网络：std::nth_element 是通用算法，对 6 个元素的开销
+            数倍于固定网络，而本函数在 1kHz 慢速任务里被 3 处调用。
+            语义与旧版一致：返回上中值（完全排序后的 [size/2]）。
 */
+static inline void median_cswap(float &a, float &b) {
+	if (a > b) {
+		float t = a;
+		a = b;
+		b = t;
+	}
+}
+
 float medium_filter(float values[], u8 size) {
-	// 使用局部数组避免动态内存分配
-	float sorted[size];
+	if (size == 6) {
+		float s0 = values[0], s1 = values[1], s2 = values[2];
+		float s3 = values[3], s4 = values[4], s5 = values[5];
+		// n=6 最优排序网络（12 个比较器，Knuth AoCP Vol.3）
+		median_cswap(s1, s2); median_cswap(s4, s5);
+		median_cswap(s0, s2); median_cswap(s3, s5);
+		median_cswap(s0, s1); median_cswap(s3, s4); median_cswap(s2, s5);
+		median_cswap(s0, s3); median_cswap(s1, s4); median_cswap(s2, s4);
+		median_cswap(s1, s3); median_cswap(s2, s3);
+		return s3;
+	}
+
+	// 其他尺寸的通用回退路径（当前无调用者）
+	float sorted[16];
+	if (size > 16U) size = 16U;
 	memcpy(sorted, values, size * SIZE_FLOAT);
-
-	// 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n)
 	std::nth_element(sorted, sorted + size / 2, sorted + size);
-
-	return sorted[size / 2]; // 返回中值
+	return sorted[size / 2];
 }
 
 // EncoderVelocityKalman类的实现

Robot_V2/Algorithm/Filter.h

@@ -7,6 +7,8 @@
 #ifndef _FILTER_H
 #define _FILTER_H
 
+
+#include "config.h"
 #ifdef __cplusplus
 extern "C" {
 #endif

Robot_V2/Algorithm/pid.cpp

@@ -12,33 +12,55 @@
 #include <cmath>
 
 extern CAN_transmiter CAN0_transmiter;
+// 微分项一阶低通离散系数：α = Ts / (τ + Ts)
+constexpr float DERIV_LPF_ALPHA = TIME_SLICE / (DERIV_LPF_TAU + TIME_SLICE);
+
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
 	  ,
 	  limit(limit) // PID控制器输出限幅
 	  ,
-	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f),
+	  measurement_prev(0.0f), derivative_filt(0.0f), deriv_init(false) {}
 
-// PID 控制器函数
-float PIDController::operator()(float error) {
+// 测量微分版本：D 项对测量量而非误差微分，目标阶跃不再产生微分踢
+float PIDController::operator()(float error, float measurement) {
+	// 首次调用只建立测量基线，避免 (meas-0)/Ts 的假微分尖峰
+	if (!deriv_init) {
+		measurement_prev = measurement;
+		deriv_init = true;
+	}
+	float d_raw = -(measurement - measurement_prev) / TIME_SLICE;
+	derivative_filt += DERIV_LPF_ALPHA * (d_raw - derivative_filt);
+	measurement_prev = measurement;
+	return step(error, D * derivative_filt);
+}
+
+// 公共计算路径：Tustin 积分 + 条件积分抗饱和 + 输出斜坡
+float PIDController::step(float error, float derivative) {
 	// P环
 	float proportional = P * error;
 
 	// Tustin 散点积分（I环）
-	// 添加积分限幅，防止积分饱和
 	float integral =
 		integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
+
+	// 将P,I,D三环的计算值加起来，与限幅比较得到未饱和/饱和输出
+	float output_unsat = proportional + integral + derivative;
+	float output = _constrain(output_unsat, -limit, limit);
+
+	// 抗积分饱和（条件积分）：输出饱和且误差仍在推高饱和方向时冻结积分。
+	// 不用大增益反算回退——深饱和时（P 项单独超限幅）反算会把积分压成反号，
+	// 误差一缩小输出瞬间反向，实测表现为到位疯狂抖动/停不到位。
+	if (output != output_unsat) {
+		if ((output_unsat > limit && error > 0.0f) ||
+			(output_unsat < -limit && error < 0.0f)) {
+			integral = integral_prev; // 冻结：不再向饱和方向累积
+		}
+	}
 	integral = _constrain(integral, -limit, limit);
 
-	// D环（微分环节）
-	float derivative = D * (error - error_prev) / TIME_SLICE;
-
-	// 将P,I,D三环的计算值加起来
-	float output = proportional + integral + derivative;
-	output = _constrain(output, -limit, limit);
-
 	// 输出斜坡限制
-	// 简化的斜坡限制（单行逻辑）
 	if (output_ramp > 0) {
 		float max_delta = output_ramp * TIME_SLICE;
 		output = output_prev +
@@ -52,6 +74,25 @@ float PIDController::operator()(float error) {
 	return output;
 }
 
+/**
+ * @brief 外部饱和泄放积分：前馈叠加后超出限幅时调用
+ * @param[in] excess 超出限幅的部分（带符号）
+ * @return void
+ * @note 仅当积分与超限同号（积分在加剧超限）时泄放，且不泄过零点：
+ *       防止前馈导致的饱和把积分抽干成反号，到位段失去保持力。
+ */
+void PIDController::bleed_integral(float excess) {
+	if (excess > 0.0f && integral_prev > 0.0f) {
+		float bleed = ANTI_WINDUP_GAIN * excess;
+		integral_prev = (integral_prev > bleed) ? (integral_prev - bleed)
+												: 0.0f;
+	} else if (excess < 0.0f && integral_prev < 0.0f) {
+		float bleed = ANTI_WINDUP_GAIN * (-excess);
+		integral_prev = (integral_prev < -bleed) ? (integral_prev + bleed)
+												 : 0.0f;
+	}
+}
+
 /**
  * @brief 设置PID参数
  *
@@ -93,6 +134,9 @@ void PIDController::reset() {
 	error_prev = 0.0f;
 	output_prev = 0.0f;
 	integral_prev = 0.0f;
+	measurement_prev = 0.0f;
+	derivative_filt = 0.0f;
+	deriv_init = false;
 }
 
 /**

Robot_V2/Algorithm/pid.h

@@ -15,9 +15,15 @@ class PIDController {
 	PIDController(float P, float I, float D, float ramp, float limit);
 	~PIDController() = default;
 
-	float operator()(float error);
 	/**
-	 * @brief 设置 set_PID 对应的参数或状态
+	 * @brief PID 计算（唯一入口）
+	 * @param[in] error 控制误差
+	 * @param[in] measurement 反馈测量量（D 项对它微分，目标阶跃不产生微分踢）
+	 * @return float PID 输出
+	 */
+	float operator()(float error, float measurement);
+	/**
+	 * @brief 设置 set_PID 对应的参数
 	 * @param[in] P 参数
 	 * @param[in] I 参数
 	 * @param[in] D 参数
@@ -25,13 +31,13 @@ class PIDController {
 	 */
 	void set_PID(float P, float I, float D);
 	/**
-	 * @brief 设置 set_P 对应的参数或状态
+	 * @brief 设置 set_P 对应的参数
 	 * @param[in] pp 参数
 	 * @return void
 	 */
 	void set_P(float pp);
 	/**
-	 * @brief 设置 set_limit 对应的参数或状态
+	 * @brief 设置 set_limit 对应的参数
 	 * @param[in] limit 参数
 	 * @return void
 	 */
@@ -46,6 +52,13 @@ class PIDController {
 	 * @return void
 	 */
 	void reset_integral(); // 仅清除积分项，保留输出斜坡状态
+	/**
+	 * @brief 外部饱和泄放积分（反算抗饱和的补充入口）
+	 * @param[in] excess 输出超出限幅的部分（带符号），如前馈叠加后超限
+	 * @return void
+	 * @note 调用方向须与 PID 输出到最终 Uq 的符号一致（有 ×DIR 的要乘上 DIR）
+	 */
+	void bleed_integral(float excess);
 
 	float P; //!< 比例增益(P环增益)
 	float I; //!< 积分增益（I环增益）
@@ -54,11 +67,20 @@ class PIDController {
 	float limit;
 
   protected:
+	/**
+	 * @brief 公共计算路径：积分、抗饱和、输出斜坡
+	 * @param[in] error 控制误差
+	 * @param[in] derivative 已完成低通滤波的微分项（D 已乘入）
+	 * @return float PID 输出
+	 */
+	float step(float error, float derivative);
+
 	float error_prev;			  //!< 最后的跟踪误差值
 	float output_prev;			  //!< 最后一个 pid 输出值
 	float integral_prev;		  //!< 最后一个积分分量值
-	unsigned long timestamp_prev; //!< 上次执行时间戳
-	float integral_separation_threshold;  //!< 积分分离阈值，用于抗积分饱和
+	float measurement_prev;		  //!< 上次测量值（对测量微分用）
+	float derivative_filt;		  //!< 微分项一阶低通滤波后的变化率
+	bool deriv_init;			  //!< 测量微分是否已完成首次采样
 };
 
 #endif