fori_zltech_motor_test/doc/04-software-control.md

04. 소프트웨어 제어 구조 (xbox_motor_control.cpp)

출처: xbox_motor_control.cpp 전체 (1382줄), Makefile, run_4wd.sh

4.1 빌드 & 실행

make                    # g++ -O3 -std=c++17 -Wall -Wextra ... -lSDL2 -lpthread -llivox_lidar_sdk_shared
./run_4wd.sh [PORT]     # USB latency_timer 1ms 튜닝 + 빌드(필요시) + 실행

run_4wd.sh는 실행 전 set_low_latency.sh로 /sys/bus/usb-serial/devices/<dev>/latency_timer를 16ms(리눅스 기본값) → 1ms로 낮춰, 100Hz 제어 루프에서 시리얼 read 지연이 병목이 되지 않게 한다.

4.2 클래스 구조

SerialPort          — 포트 open/close, Modbus RTU read/write (writeReg/writeRegs/readRegs)
MotorDriver          — SerialPort + slave_id를 감싸는 드라이버 1대(좌우 2채널) 추상화
                        initDriver / setBrakes / setRPMs / readFeedback /
                        readCurrentLimits / setMaxCurrent
LidarObstacleDetector — Livox Mid-360S SDK 콜백 기반 장애물 거리 추적 (전/후/좌/우 4방향)

드라이버 2대(driver_front, driver_rear)가 각각 MotorDriver 인스턴스로 생성되며, 브로드캐스트 모드가 아니면 후방 드라이버 포인터(driver_rear_ptr)가 유효해 개별 제어/피드백이 가능하다.

4.3 100Hz 메인 루프 개요

매 틱 (10ms):
  1. SDL 이벤트 처리 (버튼: A=트립리셋, X=라이다 토글, B/Back=종료)
  2. 조이스틱 입력 읽기 + 데드존 적용
  3. 라이다 장애물 회피 로직 (속도 스케일링 + 조향 오프셋)
  4. 스키드 스티어 운동학으로 target_fl/fr/rl/rr(RPM) 계산
  5. airborne(들뜸) 판정된 바퀴는 target를 0으로 강제
  6. 상태머신(STOPPED/RUNNING/STOPPING) + 저크 제한 S-curve로 cmd_* 갱신
  7. setRPMs()로 드라이버에 지령 전송
  8. readFeedback()으로 드라이버에서 피드백 수신
  9. 드라이버 알람(에러코드) 감지 및 콘솔 경고
  10. 휠 상태(정상/들뜸/걸림) 판정, 주행거리 갱신
  11. (옵션) CSV 로그 기록
  12. HUD 한 줄 출력 (\r로 갱신)

4.4 상태 머신

상태	전이 조건	동작
STOPPED	초기 상태	target_* 절대값이 0.1 RPM 초과하면 브레이크 해제(setBrakes(false)) 후 50ms 대기, RUNNING으로 전이
RUNNING	STOPPED에서 전이	저크 제한 프로파일로 cmd_* → target_* 추종. target_*와 cmd_*가 모두 임계값 이하면 STOPPING으로 전이
STOPPING	RUNNING에서 전이	감속 프로파일로 0으로 수렴. cmd_fl/fr이 0.1 RPM 미만이면 즉시 정지 지령(setRPMs(0,0)) 후 50ms 뒤 브레이크 잠금, STOPPED로 복귀

브레이크 해제/잠금 시 50ms 대기를 두는 이유는 전자식 브레이크(솔레노이드)의 기계적 응답 시간을 확보하기 위함이다.

4.5 저크 제한(Jerk-Limited) S-curve 프로파일

jerkLimitedStep(current_vel, target_vel, current_accel&, max_accel, max_decel, jerk_limit)

• 목표 속도까지 가속도 자체를 매 틱 jerk_limit × dt만큼만 변화시켜 부드럽게 목표 가속도에 도달시킨다. 정지 상태에서 출발할 때 첫 틱부터 풀가속도가 걸려 정지마찰이 급격히 파괴되는 문제(예: 저마찰 노면에서 순간 슬립)를 막기 위한 구조.
• 오버슈트 방지: 다음 틱 속도가 목표를 넘어서면 그 자리에서 목표값으로 스냅하고 가속도를 0으로 리셋한다.
• 가속/감속/저크 한계값은 CLI 인자로 조정 가능 (§4.8).
• 제자리 회전은 spin_jerk_rate(기본 220 RPM/s²)로 직진/커브 선회(jerk_rate, 기본 600 RPM/s²) 보다 훨씬 완만하게 적용된다 — 고마찰 노면에서 제자리 선회 시 정지마찰이 급격히 깨지지 않도록 가속에 도달하는 시간을 더 길게 늘린 것.

4.6 착지(Landing) 스냅 처리

들뜸(airborne) 상태에서 해제된(착지) 직후 한 틱은, 저크 제한 프로파일을 건너뛰고 cmd_*를 target_*로 즉시 스냅한다(just_landed_* 플래그). 차체는 이미 그 속도로 이동 중인데 지령이 정지출발용 완만한 가속 램프를 다시 타면, 그 사이 바퀴가 지면 이동 속도를 못 따라가며 끌리는 현상을 없애기 위함.

4.7 라이다(Livox Mid-360S) 장애물 회피

LidarObstacleDetector가 포인트클라우드 콜백에서 좌표를 17° 하향 피치 보정 후 4개 구역 (전방/후방/좌측/우측)의 최소 거리를 EMA(지수이동평균, 접근 시 즉각 반응·회복 시 완만)로 추적한다.

파라미터	기본값	의미
front_stop_dist	0.35 m	전방 완전 정지
front_warn_dist	0.50 m	전방 감속 시작 (선형 스케일)
side_dodge_dist	0.50 m	측면 회피 조향 시작
side_stop_dist	0.20 m	측면 회피 강도 100% 도달 거리
max_dodge_omega	0.35 rad/s	자동 회피 최대 조향 각속도

• 전진 중 전방 장애물 → 속도 스케일링(0~1) 또는 완전 정지.
• 후진 중 후방 장애물 → 완전 정지만 적용(감속 스케일링 없음).
• 전진 중이면서 사용자가 좌우 조향(rx) 입력이 없을 때만 측면 회피 조향 개입 (요청 사양: 후진 시 측면 회피 미적용).
• 라이다 데이터 800ms 이상 미수신 시 connected=false로 판단, HUD에 LIDAR:WAITING 표시.
• X 버튼으로 라이다 회피 기능 자체를 런타임에 토글 가능.

4.8 휠 이상(들뜸/걸림) 진단

속도 폐루프 특성상(부하 무관하게 지령 RPM을 추종하려 함) "지령 대비 실제속도" 하나만으로는 무부하(들뜸)를 판정할 수 없다. 두 개의 독립된 판정 축을 사용한다:

걸림/과부하 판정 (classifyStall)

vel_ratio = |실제RPM| / |지령RPM|
stall = (vel_ratio < stall_vel_ratio) && (|전류| >= stall_current_a)

지령속도가 min_active_rpm(기본 3 RPM) 미만이면 판정 보류. 기본값: stall_vel_ratio=0.5, stall_current_a=12.0A(정격 15A 근접).

무부하/들뜸 판정 (classifyAirPair)

같은 지령속도를 받는 같은쪽 앞뒤 페어(FL↔RL, FR↔RR)의 전류를 비교한다. 좌/우를 통째로 비교하면 정상 커브 선회에서 좌우 지령속도가 원래 다르기 때문에 오탐이 발생하므로, 반드시 target_fl==target_rl, target_fr==target_rr가 항상 성립하는 앞뒤 페어끼리만 비교한다.

hi = max(|amp_a|, |amp_b|)
if (hi < 0.5A) return;  // 둘 다 무전류(관성 주행)면 판정 보류
if (a < hi * airborne_current_ratio) air_a = true;   // 기본 0.35
if (b < hi * airborne_current_ratio) air_b = true;

디바운스: 노이즈성 순간 전류 편차로 즉시 개입하지 않도록, airborne_debounce_ticks (기본 5틱=50ms) 연속으로 조건이 유지될 때만 실제 airborne_* 상태가 확정되고 해당 바퀴 target이 0으로 강제된다. 접지력이 회복돼 전류가 정상화되면 다음 판정 틱에서 자동 해제되고, §4.6의 착지 스냅 로직으로 즉시 원래 지령을 재개한다.

HUD의 WHL(FL/FR/RL/RR) 4글자는 각 바퀴 상태를 O(정상)/A(들뜸,Airborne)/S(걸림,Stall)로 표시.

4.9 드라이버 알람 감지

readFeedback()로 받은 err_f_l/err_f_r/err_r_l/err_r_r 중 하나라도 0이 아니면 알람 발생으로 간주, 상태 전이 시(0→알람) 1회만 콘솔에 decodeDriverError() 결과와 함께 큰 경고를 출력한다 (매 틱 갱신되는 HUD 줄과 별개 라인). 알람 발생 시 드라이버가 명령을 무시하는 잠금 상태가 되어 소프트웨어 재시작만으로는 해제되지 않는 경우가 많음을 코드 주석이 명시 — 하드웨어 알람 클리어 또는 전원 재투입이 필요할 수 있다. 상세 대응은 05-error-troubleshooting.md 참고.

4.10 CSV 로깅 (--log <경로>)

매 틱 아래 컬럼을 기록한다:

t_ms,state,ly,lx,rx,
cmd_fl,cmd_fr,cmd_rl,cmd_rr,
fb_fl,fb_fr,fb_rl,fb_rr,
amp_fl,amp_fr,amp_rl,amp_rr,
stat_fl,stat_fr,stat_rl,stat_rr,
err_fl,err_fr,err_rl,err_rr,
dist_axle,comm_ms

실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로, 문제가 된 구간(턱 넘는 지점 등)을 나중에 잘라서 분석하기 위한 용도.

4.11 CLI 인자 전체 목록

인자	기본값	설명
--port, --port1	/dev/ttyUSB0	전방 드라이버(및 기본 후방) 시리얼 포트
--port2	(없음, port1 재사용)	후방 드라이버 전용 포트 지정 시
--id1	1	전방 드라이버 Modbus 슬레이브 주소
--id2	2	후방 드라이버 Modbus 슬레이브 주소
--bcast / --broadcast	off	두 드라이버 모두 주소 0(브로드캐스트)으로 동시 제어
--track_width	0.576 m	좌우 바퀴 중심거리
--wheelbase	0.3684 m	전후 바퀴 중심거리
--radius	0.131517 m	휠 유효 반경
--bumper	0.045 m	휠 축→전면 범퍼 거리
--accel	120 RPM/s	최대 가속도
--decel	180 RPM/s	최대 감속도
--jerk	600 RPM/s²	직진/커브 저크 한계
--spin_jerk	220 RPM/s²	제자리 회전 저크 한계
--spin_arc_bias	0.12	제자리 회전 시 혼합할 미세 전진 비율 (0~1)
--min_active_rpm	3.0 RPM	이상판정 보류 임계 지령속도
--airborne_current_ratio	0.35	무부하(들뜸) 판정 전류비
--stall_vel_ratio	0.5	걸림 판정 속도비
--stall_current_a	12.0 A	걸림 판정 전류 임계값
--airborne_debounce_ticks	5 (50ms)	들뜸 판정 디바운스
--log <path>	(비활성)	CSV 로그 파일 경로
--max_current_a	20.0 A	4바퀴 최대전류 일괄 설정 (음수=미변경)
--no_lidar	(라이다 사용)	라이다 비활성화
--lidar_config	mid360s_config.json	Livox SDK 설정 파일
--front_stop	0.35 m	전방 정지거리
--front_warn	0.50 m	전방 경고(감속)거리
--side_dodge	0.50 m	측면 회피 감지거리
--robot_width	0.410 m	라이다 회피 폭 계산용 로봇 전폭
--lidar_height	0.460 m	라이다 설치 높이
--lidar_pitch	17.0 deg	라이다 하향 피치각
--lidar_x_offset	0.250 m	라이다 전방 오프셋
--min_z / --max_z	-0.40 / 0.80 m	라이다 필터링 Z축 범위