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

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

## 4.1 빌드 & 실행

```bash
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 프로파일

```cpp
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`)

```cpp
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`가 항상 성립하는 앞뒤 페어끼리만 비교한다.

```cpp
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](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축 범위 |