speed20: 平滑线性转向消除死区断崖
This commit is contained in:
+7
-4
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user