diff --git a/run_4wd.sh b/run_4wd.sh index d706818..a00b7bb 100755 --- a/run_4wd.sh +++ b/run_4wd.sh @@ -1,7 +1,9 @@ #!/usr/bin/env bash # 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance -PORT="${1:-/dev/ttyUSB0}" +# 현재 고정 배선: 모터 드라이버 = ttyUSB1, RC 수신기(XR1) = ttyUSB0 +PORT="${1:-/dev/ttyUSB1}" +RC_PORT="${RC_PORT:-/dev/ttyUSB0}" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" cd "$SCRIPT_DIR" || exit 1 @@ -10,10 +12,11 @@ echo "==================================================================" echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템" echo "==================================================================" -# 1. USB Latency 1ms 단축 +# 1. USB Latency 1ms 단축 (모터 포트 + RC 수신기 포트 둘 다) if [ -f "./set_low_latency.sh" ]; then - echo "[1/2] USB 시리얼 포트($PORT) 지연 시간 1ms 최적화 적용 중..." + echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT)" ./set_low_latency.sh "$PORT" + ./set_low_latency.sh "$RC_PORT" fi # 2. C++ 바이너리 존재 여부 확인 및 컴파일 @@ -23,14 +26,16 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then fi echo "==================================================================" -echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작 ($PORT)" +echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작" +echo " - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT" echo " - 라이다 기능 비활성화: --no_lidar 옵션" echo " - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C" echo "==================================================================" -# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능) +# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port를 다시 넘기면 +# 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨) if [ $# -gt 1 ]; then - ./xbox_motor_control_cpp --port "$PORT" "${@:2}" + ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" "${@:2}" else - ./xbox_motor_control_cpp --port "$PORT" + ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" fi diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp index 2b4c7d8..057584f 100644 --- a/xbox_motor_control.cpp +++ b/xbox_motor_control.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -627,7 +628,7 @@ struct termios2 { // 의 커널 ABI와 동일 레이아웃 (glib }; struct RcConfig { - std::string port = "/dev/ttyUSB1"; + std::string port = "/dev/ttyUSB0"; // 현재 고정 배선: RC 수신기(XR1) = ttyUSB0 int baud = 420000; // CRSF 표준 보드레이트 int ch_steer = 1; // CH1: 좌우 조향 @@ -646,6 +647,12 @@ struct RcConfig { int brake_threshold = 1500; // 이상이면 브레이크 ON int speed_threshold = 1500; // 이상이면 저속 모드 + // CH1/CH2(연속 아날로그 스틱) 슬루레이트 한계: 메인 루프 한 틱(~10ms) + // 사이에 raw 값이 이 이상 튀면 사람이 스틱으로 낼 수 없는 변화로 보고 + // 손상된 프레임으로 간주해 버린다. CH5/CH6(스위치)는 원래 한 번에 크게 + // 뛰는 게 정상이라 이 제한을 적용하지 않는다. + int max_slew_per_tick = 200; + float v_max_low = 0.3f; // m/s (CH6 저속 모드) float v_max_high = 1.5f; // m/s (CH6 고속 모드) @@ -703,6 +710,22 @@ private: std::vector frame_buf_; + // 채널별 미디언(median-of-3) 필터 히스토리. 이 송신기는 CRC/체크섬이 + // 표준 방식으로 검증되지 않아(전수조사로 확인) 단발성 손상 프레임이 + // 섞여 들어올 수 있는데, 최근 3개 raw 값 중 가운데 값을 채택하면 3개 + // 중 하나가 손상돼도 자동으로 걸러진다. 스위치 채널도 전환 시 최대 + // 1프레임(~4ms)만 지연될 뿐이라 안전하게 전 채널에 적용 가능하다. + std::map> raw_history_; + std::map history_count_; + + static int median3(int a, int b, int c) { + if ((a <= b && b <= c) || (c <= b && b <= a)) + return b; + if ((b <= a && a <= c) || (c <= a && a <= b)) + return a; + return c; + } + static constexpr uint8_t CRSF_SYNC = 0xC8; static constexpr uint8_t CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16; static constexpr size_t CRSF_MAX_FRAME_LEN = 64; // 스펙상 프레임은 훨씬 작음 @@ -808,7 +831,21 @@ private: bit_count -= 11; int us_val = static_cast( std::lround((raw_val - 172) * 1000.0 / 1639.0 + 1000.0)); - parsed[ch + 1] = us_val; // CH1..CH16 (1-based) + + // 미디언-3 필터: 최근 2개 raw 값 + 이번 값 중 가운데 값을 채택. + // 히스토리는 항상 raw 값으로 갱신한다(필터링된 결과가 아니라). + int ch_num = ch + 1; + std::array &hist = raw_history_[ch_num]; + int &count = history_count_[ch_num]; + int filtered = us_val; + if (count >= 2) { + filtered = median3(hist[0], hist[1], us_val); + } else { + ++count; // 워밍업 구간(처음 2개)은 그대로 통과 + } + hist[0] = hist[1]; + hist[1] = us_val; + parsed[ch_num] = filtered; // CH1..CH16 (1-based) } } i += total_len; @@ -874,7 +911,7 @@ int main(int argc, char **argv) { std::signal(SIGINT, signalHandler); std::signal(SIGTERM, signalHandler); - std::string port1 = "/dev/ttyUSB0"; + std::string port1 = "/dev/ttyUSB1"; // 현재 고정 배선: 모터 드라이버 = ttyUSB1 std::string port2 = ""; uint8_t id1 = 1; uint8_t id2 = 2; @@ -884,12 +921,12 @@ int main(int argc, char **argv) { float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름 - float accel_rate = 120.0f; // RPM/s (최대 가속도 한계) + // 가속은 역기전력 보호회로가 하드웨어로 구성되어 목표값에 1:1로 즉시 + // 추종하므로 별도의 가속 한계값이 없다. decel_rate/jerk_rate(및 + // spin_jerk_rate)는 감속에만 적용된다. float decel_rate = 180.0f; // RPM/s (최대 감속도 한계) - float jerk_rate = 600.0f; // RPM/s^2 (직진/커브 선회 저크 한계) - float spin_jerk_rate = 220.0f; // RPM/s^2 (제자리 회전 저크 한계: 고마찰 노면에서 - // 정지마찰이 급격히 파괴되지 않도록 accel_rate에 - // 도달하는 시간을 더 길게 늘림) + float jerk_rate = 600.0f; // RPM/s^2 (직진/커브 선회 감속 저크 한계) + float spin_jerk_rate = 220.0f; // RPM/s^2 (제자리 회전 감속 저크 한계) float spin_arc_bias = 0.12f; // 0~1 (제자리 회전 시 섞을 최소 회전반경용 // 미세 전진 성분 비율. ICR을 로봇 중심에서 // 살짝 벗어나게 해 스크럽 마찰 저항을 줄임) @@ -951,8 +988,6 @@ int main(int argc, char **argv) { wheel_radius = std::stof(argv[++i]); else if (arg == "--bumper" && i + 1 < argc) bumper_offset = std::stof(argv[++i]); - else if (arg == "--accel" && i + 1 < argc) - accel_rate = std::stof(argv[++i]); else if (arg == "--decel" && i + 1 < argc) decel_rate = std::stof(argv[++i]); else if (arg == "--jerk" && i + 1 < argc) @@ -1031,6 +1066,8 @@ int main(int argc, char **argv) { rc_cfg.brake_threshold = std::stoi(argv[++i]); else if (arg == "--rc_speed_threshold" && i + 1 < argc) rc_cfg.speed_threshold = std::stoi(argv[++i]); + else if (arg == "--rc_max_slew" && i + 1 < argc) + rc_cfg.max_slew_per_tick = std::stoi(argv[++i]); else if (arg == "--rc_vmax_low" && i + 1 < argc) rc_cfg.v_max_low = std::stof(argv[++i]); else if (arg == "--rc_vmax_high" && i + 1 < argc) @@ -1064,7 +1101,7 @@ int main(int argc, char **argv) { << "mm\n"; std::cout << " - 최고 속도: 저속 " << rc_cfg.v_max_low << "m/s / 고속 " << rc_cfg.v_max_high << "m/s (CH" << rc_cfg.ch_speed - << " 전환) | 가속: " << accel_rate << "RPM/s | Jerk: " << jerk_rate + << " 전환) | 가속: 즉시 추종(1:1) | 감속 Jerk: " << jerk_rate << "RPM/s² (제자리회전: " << spin_jerk_rate << "RPM/s²) | 회전 곡선혼합: " << spin_arc_bias * 100.0f << "%\n"; std::cout << " - RC 수신기: " << rc_cfg.port << " @" << rc_cfg.baud @@ -1208,13 +1245,14 @@ int main(int argc, char **argv) { const float loop_hz = 100.0f; const float dt = 1.0f / loop_hz; - // 저크 제한(jerk-limited) S-curve: 가속도 자체를 매 틱 jerk_limit 만큼만 - // 변화시켜 서서히 목표 가속도에 도달하게 함. 기존 구현은 목표 접근 구간만 - // 부드러웠고 정지 상태에서 출발할 때는 첫 틱부터 거의 풀가속도가 걸려 - // 정지마찰이 급격히 파괴되는 문제가 있었음. + // 가속: 역기전력 보호회로가 하드웨어로 구성되어 더 이상 소프트웨어로 + // 완만하게 램프업할 필요가 없어져, 조이스틱 목표값에 1:1로 즉시 추종한다. + // 감속: 기존과 동일하게 저크 제한(jerk-limited) S-curve 유지 — 가속도 + // 자체를 매 틱 jerk_limit 만큼만 변화시켜 서서히 목표 가속도에 도달하게 + // 해서, 급감속 시 타이어 스크럽/전복 위험을 줄인다. auto jerkLimitedStep = [dt](float current_vel, float target_vel, - float ¤t_accel, float max_accel_mag, - float max_decel_mag, float jerk_limit) { + float ¤t_accel, float max_decel_mag, + float jerk_limit) { float diff = target_vel - current_vel; if (std::abs(diff) < 0.01f) { current_accel = 0.0f; @@ -1222,9 +1260,13 @@ int main(int argc, char **argv) { } bool is_accelerating = std::abs(target_vel) > std::abs(current_vel); - float accel_cap = is_accelerating ? max_accel_mag : max_decel_mag; + if (is_accelerating) { + current_accel = 0.0f; + return target_vel; + } + // 이번 틱에 diff를 없애는데 필요한 가속도(부호 포함)를 물리적 한계로 클램프 - float desired_accel = std::clamp(diff / dt, -accel_cap, accel_cap); + float desired_accel = std::clamp(diff / dt, -max_decel_mag, max_decel_mag); float max_delta_accel = jerk_limit * dt; current_accel = std::clamp(desired_accel, current_accel - max_delta_accel, @@ -1249,9 +1291,9 @@ int main(int argc, char **argv) { 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으로 - // 스냅시켜 지연을 없앤다. + // 있는데 cmd가 뒤늦게 target을 쫓아가면 그 사이 바퀴가 지면 이동속도를 + // 못 따라가 끌리며 긁히는 문제가 있어 즉시 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"; @@ -1260,6 +1302,18 @@ int main(int argc, char **argv) { bool prev_rc_ok = false; // RC 연결 직전 틱 상태 (Fail-safe 로그용) float omega_max = (effective_w > 0.0f) ? (max_spin_v / (effective_w / 2.0f)) : 0.0f; // CH1 조향 최대 각속도 + // CRC 미검증으로 인해 손상된 CRSF 프레임이 드물게 통과할 수 있다. + // (실측 결과 이 송신기는 표준 CRC8 변형 어떤 것과도 안 맞아 — poly + // 전수조사+여러 바이트 범위로 확인해도 일치율이 최대 33% 수준이라 + // CRC 자체를 신뢰할 수 없다고 판단, 검증 시도를 포기함.) 대신 두 가지 + // 물리적 타당성 검사로 걸러낸다: + // 1) 캘리브레이션 범위(steer_min~max, throttle_min~max) 밖의 값 거부 + // 2) 슬루레이트: 한 틱(~10ms) 사이에 사람이 스틱으로 낼 수 없을 만큼 + // 크게 튀는 값 거부 (예: 1227 -> 1800 같은 순간 점프) + // 두 경우 모두 그 틱은 버리고 마지막으로 유효했던 값을 그대로 사용한다. + int last_raw_steer = rc_cfg.steer_center_lo; + int last_raw_throttle = 1500; + bool steer_initialized = false, throttle_initialized = false; while (g_running) { // --------------------------------------------------------------------- @@ -1273,6 +1327,30 @@ int main(int argc, char **argv) { rc.getChannel(rc_cfg.ch_brake, raw_brake); rc.getChannel(rc_cfg.ch_speed, raw_speed_mode); + bool steer_range_ok = + raw_steer >= rc_cfg.steer_min && raw_steer <= rc_cfg.steer_max; + bool steer_slew_ok = !steer_initialized || + std::abs(raw_steer - last_raw_steer) <= + rc_cfg.max_slew_per_tick; + if (steer_range_ok && steer_slew_ok) { + last_raw_steer = raw_steer; + steer_initialized = true; + } else { + raw_steer = last_raw_steer; // 범위 밖 또는 비정상 점프 -> 손상된 프레임으로 간주 + } + + bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min && + raw_throttle <= rc_cfg.throttle_max; + bool throttle_slew_ok = !throttle_initialized || + std::abs(raw_throttle - last_raw_throttle) <= + rc_cfg.max_slew_per_tick; + if (throttle_range_ok && throttle_slew_ok) { + last_raw_throttle = raw_throttle; + throttle_initialized = true; + } else { + raw_throttle = last_raw_throttle; + } + // CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로 // 직접 눈으로 검증할 수 있도록 HUD/CSV에 그대로 노출). int raw_ch[9]; // 인덱스 1..8 사용 @@ -1442,6 +1520,8 @@ int main(int argc, char **argv) { // 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시 // 0으로 스냅 후 브레이크를 잠근다. 해제되면 STOPPED 상태에서 아래 // 상태머신이 정상적으로 재가속을 시작한다. + // 전자식 브레이크(솔레노이드)는 오직 CH5로만 제어한다 — 스로틀을 중립으로 + // 되돌려 자연 정지했을 때는 잠그지 않고, CH5를 눌렀을 때만 잠근다. if (brake_on && !prev_brake_on) { cmd_fl = cmd_fr = cmd_rl = cmd_rr = 0.0f; accel_fl = accel_fr = accel_rl = accel_rr = 0.0f; @@ -1453,6 +1533,10 @@ int main(int argc, char **argv) { if (driver_rear_ptr) driver_rear_ptr->setBrakes(true); state = "STOPPED"; + } else if (!brake_on && prev_brake_on) { + driver_front.setBrakes(false); + if (driver_rear_ptr) + driver_rear_ptr->setBrakes(false); } prev_brake_on = brake_on; @@ -1460,10 +1544,6 @@ int main(int argc, char **argv) { // target_fl..rr은 이미 0(v_x=omega=0)이며 STOPPED 상태를 유지한다. } else if (state == "STOPPED") { if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) { - driver_front.setBrakes(false); - if (driver_rear_ptr) - driver_rear_ptr->setBrakes(false); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); state = "RUNNING"; } } else if (state == "RUNNING") { @@ -1491,14 +1571,10 @@ int main(int argc, char **argv) { } 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); - cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, accel_rate, - decel_rate, jerk_now); - cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, accel_rate, - decel_rate, jerk_now); - cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, accel_rate, - decel_rate, jerk_now); + cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate, jerk_now); + cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now); + cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now); + cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, decel_rate, jerk_now); if (std::abs(target_fl) < 0.1f && std::abs(target_fr) < 0.1f && std::abs(cmd_fl) < 0.5f && std::abs(cmd_fr) < 0.5f) { @@ -1506,14 +1582,10 @@ int main(int argc, char **argv) { } } else if (state == "STOPPING") { float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate; - cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate, - decel_rate, jerk_now); - cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, - decel_rate, jerk_now); - cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, - decel_rate, jerk_now); - cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, decel_rate, - decel_rate, jerk_now); + cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate, jerk_now); + cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now); + cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now); + cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, decel_rate, jerk_now); if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) { cmd_fl = 0.0f; @@ -1524,10 +1596,8 @@ int main(int argc, char **argv) { driver_front.setRPMs(0, 0); if (driver_rear_ptr) driver_rear_ptr->setRPMs(0, 0); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - driver_front.setBrakes(true); - if (driver_rear_ptr) - driver_rear_ptr->setBrakes(true); + // 스로틀 중립 복귀로 인한 자연 정지: 전자식 브레이크는 잠그지 않는다 + // (CH5를 눌렀을 때만 잠금 — 위 브레이크 최우선 로직 참고). state = "STOPPED"; } } diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp index b0d4d9b..aa2fe86 100755 Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ