speed20: 平滑线性转向消除死区断崖

This commit is contained in:
spdis
2026-07-06 17:25:24 +08:00
parent afc839e8cb
commit a43ff769d4
+7 -4
View File
@@ -962,12 +962,15 @@ static void steering_update()
}
}
if (std::abs(deviation) < g_cfg.deadband) {
// 死区内 → 直行
// 平滑线性转向: 偏差从死区边界开始缩放,消除死区断崖
double effective = std::abs(deviation) - g_cfg.deadband;
if (effective <= 0) {
servo.setDutyCycle(1500000);
} else {
// 比例控制: 偏差 → 归一化 → × 增益 × 满偏占空比差
double offset = deviation / (newWidth / 2.0) * g_cfg.steer_gain * 300000;
double half_w = newWidth / 2.0;
double norm = effective / (half_w - g_cfg.deadband);
double sign = (deviation > 0) ? 1.0 : -1.0;
double offset = norm * g_cfg.steer_gain * 300000.0 * sign;
double duty_ns = std::clamp(1500000.0 + offset, 1200000.0, 1800000.0);
servo.setDutyCycle((unsigned int)duty_ns);
}