2 Commits

Author SHA1 Message Date
robin ed34e1c025 Snap wheel command to target immediately on landing after airborne
Previously, once a wheel's airborne flag cleared, its target reverted
to the normal drive command but the actual RPM still had to ramp up
through the gentle standstill-launch accel_rate/jerk profile. Since
the chassis is already moving at that point (unlike a stop-start),
the wheel spent several hundred ms unable to match ground speed,
dragging/grinding against the surface and occasionally landing on a
momentarily negative curve-turn target, making it spin backward.

Track the airborne->grounded transition per wheel and, on the tick it
clears, snap cmd directly to target and reset the jerk accel state
instead of ramping, so the wheel rejoins the other three immediately.

Verified working on hardware.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
2026-08-13 13:22:29 +09:00
robin 19fb70d998 Fix curve-steering wheel-reversal at partial throttle
Steering angular velocity (omega) was scaled by the constant max
speed (max_v) instead of the actual current forward speed (v_x).
This decoupled turn tightness from throttle position: at low/medium
throttle, a moderate left-stick steering input already produced an
omega large enough to flip the inner wheel's commanded velocity
negative, causing the robot to spin/reverse/lurch forward instead of
smoothly curving. Scaling by abs(v_x) keeps turn radius proportional
to speed as intended, matching prior full-throttle behavior while
fixing the reversal at partial throttle.

Also drops the stale committed x86-64 binary (built before this fix,
so its behavior no longer matched source). Build fresh on an x86
machine via `make`; run_4wd.sh already rebuilds automatically when
the binary is missing.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
2026-08-13 10:56:33 +09:00
2 changed files with 53 additions and 1 deletions
+53 -1
View File
@@ -942,6 +942,12 @@ int main(int argc, char **argv) {
bool airborne_fl = false, airborne_fr = false; bool airborne_fl = false, airborne_fr = false;
bool airborne_rl = false, airborne_rr = false; bool airborne_rl = false, airborne_rr = false;
int air_count_fl = 0, air_count_fr = 0, air_count_rl = 0, air_count_rr = 0; int air_count_fl = 0, air_count_fr = 0, air_count_rl = 0, air_count_rr = 0;
// 착지(airborne 해제) 직후 한 틱 동안: 차체는 이미 그 속도로 움직이고
// 있는데 cmd는 정지출발용 accel_rate로 천천히 재가속되면, 그 사이 바퀴가
// 지면 이동속도를 못 따라가 끌리며 긁히는 문제가 있어 즉시 target으로
// 스냅시켜 지연을 없앤다.
bool just_landed_fl = false, just_landed_fr = false;
bool just_landed_rl = false, just_landed_rr = false;
char wheel_status_str[5] = "OOOO"; char wheel_status_str[5] = "OOOO";
bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부 bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부
@@ -1078,7 +1084,14 @@ int main(int argc, char **argv) {
target_rl = v_l * rpm_per_ms; target_rl = v_l * rpm_per_ms;
target_rr = -v_r * rpm_per_ms; target_rr = -v_r * rpm_per_ms;
} else { } else {
omega += -lx * (max_v / (effective_w / 2.0f)); // 선회 강도(omega)는 반드시 "현재 전진 속도(v_x)"에 비례해야 커브
// 반경이 스로틀과 무관하게 일정하게 유지된다. 이전 구현은 상수인
// max_v(최고속도)를 기준으로 omega를 계산해서, 저속 주행 중에는
// v_x가 작은데도 omega*k_skid 항은 고속 기준 그대로 커서 안쪽
// 바퀴(v_l 또는 v_r) 속도 부호가 뒤집혀 후진해버리는 문제가 있었다.
// (예: 저속으로 스틱을 왼쪽으로 조금만 꺾어도 로봇이 제자리 회전
// 하듯 튀거나 뒤로 갔다 앞으로 갔다 하는 증상)
omega += -lx * (std::abs(v_x) / (effective_w / 2.0f));
float v_l = v_x - omega * k_skid; float v_l = v_x - omega * k_skid;
float v_r = v_x + omega * k_skid; float v_r = v_x + omega * k_skid;
@@ -1110,6 +1123,29 @@ int main(int argc, char **argv) {
state = "RUNNING"; state = "RUNNING";
} }
} else if (state == "RUNNING") { } else if (state == "RUNNING") {
// 착지 직후: 정지출발용 저크 램프를 건너뛰고 차체가 이미 내고 있는
// 속도로 즉시 맞춰 지면과의 미끄러짐(끌림)을 최소화한다.
if (just_landed_fl) {
cmd_fl = target_fl;
accel_fl = 0.0f;
just_landed_fl = false;
}
if (just_landed_fr) {
cmd_fr = target_fr;
accel_fr = 0.0f;
just_landed_fr = false;
}
if (just_landed_rl) {
cmd_rl = target_rl;
accel_rl = 0.0f;
just_landed_rl = false;
}
if (just_landed_rr) {
cmd_rr = target_rr;
accel_rr = 0.0f;
just_landed_rr = false;
}
float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate; float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate;
cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, accel_rate, cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, accel_rate,
decel_rate, jerk_now); decel_rate, jerk_now);
@@ -1236,11 +1272,27 @@ int main(int argc, char **argv) {
air_count_rl = air_now_rl ? air_count_rl + 1 : 0; air_count_rl = air_now_rl ? air_count_rl + 1 : 0;
air_count_rr = air_now_rr ? air_count_rr + 1 : 0; air_count_rr = air_now_rr ? air_count_rr + 1 : 0;
bool prev_airborne_fl = airborne_fl;
bool prev_airborne_fr = airborne_fr;
bool prev_airborne_rl = airborne_rl;
bool prev_airborne_rr = airborne_rr;
airborne_fl = air_count_fl >= airborne_debounce_ticks; airborne_fl = air_count_fl >= airborne_debounce_ticks;
airborne_fr = air_count_fr >= airborne_debounce_ticks; airborne_fr = air_count_fr >= airborne_debounce_ticks;
airborne_rl = air_count_rl >= airborne_debounce_ticks; airborne_rl = air_count_rl >= airborne_debounce_ticks;
airborne_rr = air_count_rr >= airborne_debounce_ticks; airborne_rr = air_count_rr >= airborne_debounce_ticks;
// 방금 착지(뜬 상태 -> 정상)한 바퀴는 다음 틱에 cmd를 target으로 즉시
// 스냅해 재가속 지연을 없앤다.
if (prev_airborne_fl && !airborne_fl)
just_landed_fl = true;
if (prev_airborne_fr && !airborne_fr)
just_landed_fr = true;
if (prev_airborne_rl && !airborne_rl)
just_landed_rl = true;
if (prev_airborne_rr && !airborne_rr)
just_landed_rr = true;
auto statusChar = [](bool air, bool stall) { auto statusChar = [](bool air, bool stall) {
return air ? 'A' : (stall ? 'S' : 'O'); return air ? 'A' : (stall ? 'S' : 'O');
}; };
Binary file not shown.