diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp index 49b60f9..a76b5cc 100644 --- a/xbox_motor_control.cpp +++ b/xbox_motor_control.cpp @@ -942,6 +942,12 @@ int main(int argc, char **argv) { bool airborne_fl = false, airborne_fr = 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; + // 착지(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"; bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부 @@ -1117,6 +1123,29 @@ int main(int argc, char **argv) { 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; cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, accel_rate, decel_rate, jerk_now); @@ -1243,11 +1272,27 @@ int main(int argc, char **argv) { air_count_rl = air_now_rl ? air_count_rl + 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_fr = air_count_fr >= airborne_debounce_ticks; airborne_rl = air_count_rl >= 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) { return air ? 'A' : (stall ? 'S' : 'O'); }; diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp new file mode 100755 index 0000000..417afb1 Binary files /dev/null and b/xbox_motor_control_cpp differ