Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 robin
|
dec1d85f9b | ||
|
robin
|
6be30eb929 | ||
|
robin
|
bb5a0adc0c | ||
 Dongubak
|
dd77dbf1bb | ||
|
Dongubak
|
2f21d86193 |
@@ -1,6 +1,6 @@
|
|||||||
CXX = g++
|
CXX = g++
|
||||||
CXXFLAGS = -O3 -std=c++17 -Wall -Wextra -I/usr/local/include
|
CXXFLAGS = -O3 -std=c++17 -Wall -Wextra -I/usr/local/include
|
||||||
LIBS = -lSDL2 -lpthread -llivox_lidar_sdk_shared
|
LIBS = -lpthread -llivox_lidar_sdk_shared
|
||||||
|
|
||||||
TARGET = xbox_motor_control_cpp
|
TARGET = xbox_motor_control_cpp
|
||||||
SRC = xbox_motor_control.cpp
|
SRC = xbox_motor_control.cpp
|
||||||
|
|||||||
@@ -0,0 +1,187 @@
|
|||||||
|
## A (q1.md에 대한 답변)
|
||||||
|
|
||||||
|
`doc/06-imu-integration-plan.md`에 대한 4가지 질문에 순서대로 답한다. 결론을 먼저 요약하면:
|
||||||
|
**개루프(조이스틱)와 폐루프(IMU 트림)는 상충하지 않으며 실무에서도 계층 구조로 함께 쓰인다.
|
||||||
|
ICR은 실제로 동적이며 9축 IMU로 온라인 추정이 가능하다. 헤딩 홀드는 상대 헤딩 기준 + 최소한의
|
||||||
|
필터링으로 충분하다. HWT905는 RS485/Modbus를 지원해 기존 통신 스택 재사용이 가능하다.**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Q1. 조이스틱 폐루프 제어가 실무에서 실제로 쓰이는가?
|
||||||
|
|
||||||
|
**결론: 쓰인다. 다만 사용자가 걱정하는 "조이스틱 값이 바뀌면 폐루프 계산을 처음부터 다시 해야
|
||||||
|
한다"는 문제는 애초에 발생하지 않는다.** 그 걱정 자체가 폐루프 제어에 대한 흔한 오해를 짚고
|
||||||
|
있어서, 왜 문제가 안 되는지부터 설명한다.
|
||||||
|
|
||||||
|
#### 왜 "재계산 문제"가 없는가
|
||||||
|
|
||||||
|
폐루프 제어의 기본 형태는 다음과 같다.
|
||||||
|
|
||||||
|
```
|
||||||
|
e(t) = r(t) − y(t) // r(t): 기준값(레퍼런스), y(t): 센서 측정값
|
||||||
|
u(t) = Controller(e(t)) // 그 순간의 오차만 보고 그 순간의 보정량을 계산
|
||||||
|
```
|
||||||
|
|
||||||
|
여기서 `r(t)`(조이스틱이 매 순간 주는 목표값)가 틱마다 바뀌는 것은 폐루프 제어의 정상적인
|
||||||
|
동작 방식이지, 예외 상황이 아니다. 제어기는 "이전 계산이 끝날 때까지 기다렸다가 다음 값을
|
||||||
|
반영"하는 게 아니라, **매 사이클(본 프로젝트는 100Hz=10ms)마다 그 순간의 `r(t)`와 `y(t)`만으로
|
||||||
|
그 순간의 보정량 하나를 계산**한다. 지금 이미 코드에 있는 `jerkLimitedStep()`도 정확히 이 구조다
|
||||||
|
— `target_*`(조이스틱에서 매 틱 갱신되는 레퍼런스)가 바뀌어도 다음 틱에서 그냥 새 `target_*`
|
||||||
|
기준으로 다시 한 스텝 계산할 뿐, "이전 목표를 향한 계산이 안 끝났으니 못 바꾼다"는 일이 없다.
|
||||||
|
헤딩 홀드도 동일한 구조로 넣을 수 있다.
|
||||||
|
|
||||||
|
#### 이미 이 프로젝트에도 폐루프가 존재한다
|
||||||
|
|
||||||
|
"조이스틱 제어 = 개루프"라는 인식은 절반만 맞다. 계층을 나눠보면:
|
||||||
|
|
||||||
|
| 계층 | 신호 흐름 | 개/폐루프 |
|
||||||
|
| :--- | :--- | :-: |
|
||||||
|
| 상위 (조이스틱 → 목표 RPM) | `ly/lx/rx` → 스키드 스티어 킨매틱스 → `target_fl/fr/rl/rr` | **개루프** (피드포워드) |
|
||||||
|
| 중위 (목표 RPM → 지령 RPM) | 저크 제한 S-curve (`jerkLimitedStep`) | 개루프 프로파일 생성 (폐루프 아님, 단 매 틱 최신 목표 반영) |
|
||||||
|
| **하위 (지령 RPM → 실제 RPM)** | **드라이버 내장 Velocity Loop** (`0x203C`/`0x203D`/`0x203E` L, `0x206C`/`0x206D`/`0x206E` R — Kp/Ki/Kf) | **이미 폐루프.** 부하가 걸려도 드라이버가 실제 RPM을 지령 RPM에 맞추려 함 |
|
||||||
|
|
||||||
|
즉 지금도 "조이스틱 값 → 목표 속도"까지는 개루프지만, "목표 속도 → 실제 바퀴 속도"는 이미
|
||||||
|
드라이버가 폐루프로 하고 있다. IMU 헤딩 홀드(계획서 Phase 3)는 이 구조에 **한 계층을 더
|
||||||
|
얹는 것**뿐이다: "조향 입력이 중립일 때, 목표 요레이트(=0)와 실측 요레이트(IMU)의 오차를 보고
|
||||||
|
좌/우 목표 RPM에 작은 트림을 더한다." 조이스틱이 다시 움직이면(조향 입력 재개) 그 즉시 헤딩
|
||||||
|
락을 풀고 트림을 0으로 되돌리면 되므로, "조이스틱 값 변경 시 재계산" 문제는 설계상 발생하지
|
||||||
|
않는다.
|
||||||
|
|
||||||
|
#### 실무 사례 (동일 패턴)
|
||||||
|
|
||||||
|
* **자동차 ESC/TCS(전자식 주행안정장치)**: 운전자의 스티어링·가속페달이 항상 최우선 레퍼런스지만,
|
||||||
|
요레이트 센서가 실측한 값이 스티어링 각도로부터 기대되는 값과 어긋나면 개별 바퀴 브레이크/토크를
|
||||||
|
트림한다. 운전자가 스티어링을 계속 바꿔도 문제없이 동작 — 매 사이클 그 순간의 편차만 본다.
|
||||||
|
* **쿼드콥터**: 스틱은 각속도/자세에 대한 레퍼런스이고, 내부 자세제어 루프가 IMU로 안정화한다.
|
||||||
|
스틱을 중립으로 놓으면 자세/포지션 홀드로 전환되어 바람 등 외란을 자동 보정 — Phase 3와 정확히
|
||||||
|
동일한 "중립 시에만 홀드" 패턴.
|
||||||
|
* **선박 오토파일럿**: 조타핸들이 중립일 때만 자이로컴퍼스 기준 헤딩홀드가 개입하고, 조타를
|
||||||
|
움직이면 즉시 새 헤딩으로 재설정된다.
|
||||||
|
|
||||||
|
**요약**: 실무의 표준 패턴은 "조이스틱 = 상위 레퍼런스(운전자 의도, 매 틱 갱신 가능) + 하위
|
||||||
|
폐루프(그 레퍼런스를 추종·안정화)"의 계층 구조다. 계획서 Phase 3가 제안하는 헤딩 홀드도 이
|
||||||
|
패턴을 그대로 따른다.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Q2. ICR이 동적이라면, IMU로 이를 감지해 더 고차원의 제어가 가능한가?
|
||||||
|
|
||||||
|
**결론: 가능하다. 그리고 지적한 대로 현재 로직은 "동적으로 변하는 현상을 정적 스칼라로 땜빵"한
|
||||||
|
것이 맞다.**
|
||||||
|
|
||||||
|
#### 현재 로직이 "땜빵"인 이유 (재확인)
|
||||||
|
|
||||||
|
`k_skid`, `effective_w`는 로봇 형상(트랙폭, 축거)만으로 계산된 기하학적 근사치이고,
|
||||||
|
`spin_arc_bias=0.12`는 실측 없이 경험으로 넣은 상수다. 이 값들은 지면 마찰계수, 무게중심,
|
||||||
|
경사, 타이어 트레드 마모, 속도에 따라 실제로는 계속 바뀌는 "유효 ICR"을 대표할 수 없다
|
||||||
|
— 특정 노면/속도 조건에서 튜닝된 값일 뿐이다.
|
||||||
|
|
||||||
|
#### 9축 IMU가 추가하는 정보 차원
|
||||||
|
|
||||||
|
| IMU 축 | 얻는 정보 | 현재(전류+엔코더)로는 불가능한 이유 |
|
||||||
|
| :--- | :--- | :--- |
|
||||||
|
| 자이로 요레이트 | 로봇이 **실제로** 얼마나 회전하고 있는가 (슬립 무관) | 전류는 부하/토크 정보, 엔코더는 개별 바퀴 회전량 정보일 뿐 — 차체가 실제로 얼마나 돌았는지는 아무도 안 알려준다 |
|
||||||
|
| 가속도계 (중력보정 후) | 급조향/급가감속 시 차체가 실제로 그만큼 가속했는가 | 저크 리미터가 만드는 "기대 가속도"와 비교해 "명령은 갔는데 차체가 안 따라옴" 상태(전신 슬립)를 검출 가능 |
|
||||||
|
| Roll/Pitch(경사각) | 현재 지형이 경사면인지 | 경사에서는 좌우 바퀴 접지압이 비대칭이 되어 ICR이 더 크게 틀어짐 — 지금은 이 정보 자체가 전무 |
|
||||||
|
| 지자기 | 절대 방위 참조 (제한적) | 다만 차체가 모터/드라이버/배터리 등 금속 덩어리라 지자기 왜곡이 클 가능성이 높음 — 절대 헤딩보다는 참고용으로만 신중히 사용 권장 |
|
||||||
|
|
||||||
|
#### "정적 캘리브레이션"보다 한 단계 더 — 온라인 ICR 추정
|
||||||
|
|
||||||
|
계획서 Phase 5(§6.4)는 원래 "몇 차례 실측 후 한 번 피팅해서 상수를 교체"하는 **정적**
|
||||||
|
캘리브레이션으로 설계했다. 하지만 지적한 대로 ICR이 지형에 따라 계속 바뀐다면, 매 틱
|
||||||
|
|
||||||
|
```
|
||||||
|
effective_ICR_gain = 실측_요레이트(IMU) / 지령_차동속도
|
||||||
|
```
|
||||||
|
|
||||||
|
를 계속 갱신하는 **온라인 추정**(Pentzer 2014가 제안한 방식)이 이론적으로는 더 맞는 방향이다.
|
||||||
|
다만 이건 Phase 5를 정적 캘리브레이션 → 상시 온라인 추정으로 확장하는 것이므로, 구현 난이도와
|
||||||
|
실외 검증 부담이 커진다. 이번 답변을 반영해 계획서 §6.4 Phase 5에 "1차: 정적 캘리브레이션으로
|
||||||
|
먼저 검증 → 2차: 온라인 추정으로 확장 가능"이라는 단계를 추가해 두었다(아래 "계획서 반영"
|
||||||
|
참고).
|
||||||
|
|
||||||
|
#### 우선순위: 조이스틱 제어가 먼저라는 원칙과의 정합성
|
||||||
|
|
||||||
|
이 답변에서 제안하는 내용은 계획서의 Phase 순서를 바꾸지 않는다. Phase 1~2(§6.4)는 IMU 값을
|
||||||
|
"관측/로깅"만 할 뿐 제어에 전혀 개입하지 않으므로, 조이스틱 제어 자체의 완성도에는 영향을 주지
|
||||||
|
않는다. ICR 온라인 추정 같은 고차원 활용은 Phase 5 이후, 즉 조이스틱 경로가 충분히 검증된
|
||||||
|
다음에야 의미가 있는 단계로 이미 순서가 잡혀 있다.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Q3. 헤딩 기준을 IMU로 얻어 직진 편향 보정이 가능한가? 필터는?
|
||||||
|
|
||||||
|
**결론: 맞다. 그리고 이 용도에서는 "정밀한 절대 헤딩"이 아니라 "짧은 구간 동안의 상대 헤딩"만
|
||||||
|
있으면 되기 때문에, 필터링 요구 수준이 생각보다 무겁지 않다.**
|
||||||
|
|
||||||
|
#### 왜 절대 헤딩이 필요 없는가
|
||||||
|
|
||||||
|
Phase 3(§6.4)의 헤딩 홀드는 "조향 입력이 중립인 동안 로봇이 옆으로 틀어지지 않게" 하는 용도다.
|
||||||
|
조향 입력이 다시 들어오면(사용자가 스틱을 움직이면) 헤딩 락을 풀고, 다시 중립이 되는 시점에
|
||||||
|
**그 순간의 방향을 새 기준으로 재설정(reset)**하면 된다. 즉 각 "직진 구간"은 보통 수 초~수십 초
|
||||||
|
단위이므로, 자이로 바이어스 드리프트가 누적될 시간 자체가 짧아 실사용상 큰 문제가 되지 않는다.
|
||||||
|
(장기 절대 헤딩이 필요해지는 건 계획서 Phase 6의 자율주행/맵핑 단계부터다.)
|
||||||
|
|
||||||
|
#### 그래도 필요한 최소한의 처리
|
||||||
|
|
||||||
|
1. **정적 바이어스 캘리브레이션**: 전원 인가 직후 정지 상태에서 수 초간 자이로 값을 평균 내
|
||||||
|
영점(zero-rate offset)을 구해 빼준다. 이거 없이는 정지 중에도 헤딩이 서서히 흘러간다.
|
||||||
|
2. **노이즈 필터링**: 실외 오프로드 특성상 진동이 크다. 저역통과 또는 이동평균 정도로 고주파
|
||||||
|
진동 성분을 제거해야 트림 제어기가 노이즈에 반응해 떨지 않는다.
|
||||||
|
3. **HWT905류 온보드 AHRS 모듈을 쓰면 위 부담이 상당 부분 줄어든다**: 이 모듈은 가속도+자이로
|
||||||
|
+지자기를 내부적으로 칼만필터 등으로 융합해 이미 안정화된 roll/pitch/yaw를 직접 출력한다.
|
||||||
|
순수 MEMS 칩(raw 자이로 값만 주는 IC)을 쓰는 것보다 애플리케이션 쪽 필터링 구현 부담이
|
||||||
|
훨씬 적다 — Q4에서 이어서 설명.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Q4. IMU 하드웨어: HWT905 확정
|
||||||
|
|
||||||
|
WitMotion 공식 자료 기준으로 확인한 HWT905-RS485 스펙 (정식 PDF 매뉴얼은 이미지 기반이라
|
||||||
|
텍스트 자동 추출이 안 돼, 아래는 제조사 웹페이지 기준 요약이다 — 레지스터 맵 등 세부 사항은
|
||||||
|
§실행 전 확인 필요 참고):
|
||||||
|
|
||||||
|
| 항목 | 내용 |
|
||||||
|
| :--- | :--- |
|
||||||
|
| 측정 축 | 9축 — 가속도 3축 + 자이로 3축 + 지자기 3축, 내장 자세융합 알고리즘으로 roll/pitch/yaw 직접 출력 |
|
||||||
|
| 통신 | RS485, **Modbus 프로토콜 지원**(레지스터 리스트 기반) |
|
||||||
|
| 보드레이트 | 기본 9600bps, 2400~921600bps 가변 |
|
||||||
|
| 출력 레이트 | 최대 200Hz |
|
||||||
|
| 정확도 | 제조사 공식 스펙 "0.05°" (동적/정적 조건은 정식 PDF 확보 후 재확인 필요) |
|
||||||
|
| 케이스 | 55×36.8×24mm 알루미늄, **IP67 방수/방진** |
|
||||||
|
| 케이블 | RS485 최대 30m |
|
||||||
|
|
||||||
|
#### 이 프로젝트에 잘 맞는 이유
|
||||||
|
|
||||||
|
1. **RS485 + Modbus 프로토콜** — `xbox_motor_control.cpp`의 `SerialPort` 클래스(Modbus RTU,
|
||||||
|
CRC16, `readReg`/`writeReg`/`readRegs` 패턴)를 상당 부분 재사용할 수 있다. IMU만을 위한
|
||||||
|
완전히 새로운 통신 스택을 만들 필요가 없다. (단, ZLAC8015D 2대가 이미 쓰고 있는 버스에
|
||||||
|
같이 물릴지, 별도 물리 포트로 분리할지는 §실행 전 확인 필요에서 결정해야 한다 — 100Hz
|
||||||
|
루프에서 통신 3개 슬레이브를 한 버스로 돌리면 틱당 통신 시간이 늘어날 수 있다.)
|
||||||
|
2. **IP67 방수/방진** — 실외 로봇이라는 조건에 부합.
|
||||||
|
3. **온보드 AHRS 융합** — Q3에서 설명한 필터링 부담을 하드웨어가 상당 부분 대신 처리해준다.
|
||||||
|
저가 MEMS 칩 대비 확실한 이점.
|
||||||
|
|
||||||
|
#### 실행 전 확인 필요 (아직 확정 못한 것)
|
||||||
|
|
||||||
|
이번 조사에서 확보한 HWT905 정식 매뉴얼 PDF는 이미지 스캔본이라 텍스트 추출이 되지 않아,
|
||||||
|
**정확한 레지스터 주소(각속도/각도/가속도/지자기 레지스터, 출력레이트 설정 레지스터, 슬레이브
|
||||||
|
주소 설정 방법 등)는 이번 문서에 포함하지 못했다.** ZLAC8015D 때와 동일한 방식으로 진행하려면:
|
||||||
|
|
||||||
|
1. HWT905 정식 매뉴얼(텍스트 검색 가능한 PDF)을 `resource/`에 추가.
|
||||||
|
2. `doc/07-imu-hwt905-spec.md`(가칭)로 레지스터 맵을 ZLAC8015D 문서와 동일한 정밀도로 문서화.
|
||||||
|
3. 그 다음에야 Phase 1(하드웨어 연동) 코드 작업 착수.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 계획서(`doc/06-imu-integration-plan.md`) 반영 사항
|
||||||
|
|
||||||
|
이 답변 내용을 반영해 계획서를 다음과 같이 갱신했다:
|
||||||
|
|
||||||
|
* §6.7 "결정이 필요한 사항" 중 **IMU 하드웨어 항목을 HWT905로 확정** 표시.
|
||||||
|
* Phase 5에 "정적 캘리브레이션 → 온라인 ICR 추정으로 확장 가능" 단계 구분을 추가.
|
||||||
|
* Phase 1 앞에 "IMU 정식 매뉴얼 확보 및 레지스터 맵 문서화"를 선행 작업으로 명시.
|
||||||
|
|
||||||
|
남은 결정 사항: **IMU를 ZLAC8015D와 같은 RS485 버스에 물릴지, 별도 포트로 분리할지** — 이건
|
||||||
|
실측(버스 하나에 3개 슬레이브 얹었을 때 100Hz 루프의 `comm_ms` 여유가 있는지) 후 결정하는 것을
|
||||||
|
제안한다.
|
||||||
@@ -0,0 +1,11 @@
|
|||||||
|
## Q
|
||||||
|
06-imu-integration-plan.md 문서를 읽어보았습니다.
|
||||||
|
|
||||||
|
현재 코드를 분석한 결과 제가 알 수 있는 것은 다음과 같았습니다.
|
||||||
|
- 현재 개루프 텔레옵이다. 하지만 조이스틱으로의 제어를 먼저 완성하려는 입장에서 스키드 스티어 차량의 경우 개루프일 수 밖에 없는 것이 아닌가. 가령 폐루프를 그린다고 하면 폐루프 계산을 진행하고 다음 제어를 할 때 조이스틱의 값이 변경되면 다시 계산해야 할텐데, 나도 이런 제어방식이 있어야 완벽한 조이스틱 제어가 완성될 것이라고 생각한다. 하지만 실무에서 실제로 조이스틱 제어가 폐루프로 이루어지는 지 알고 싶다.
|
||||||
|
|
||||||
|
- ICR은 로봇이 있는 곳의 경사 그리고 무게중심, 지형 특성, 바퀴의 트레드 등 여러 요소에 의존되어 다이나믹하게 변할 것이다. 현재 로직은 분석한 문서를 보면, 변화무쌍한 ICR을 고정된 스칼라값을 입력하여 땜빵식으로의 보정을 한 것이라고 이해하였다. 그렇다면 IMU를 추가하여 ICR을 다이나믹하게 감지하여 전류와 현재 회전량 뿐만아니라 9축 IMU를 통해 고차원의 제어가 가능한 것인가. 일단 조이스틱 제어가 우선이다.
|
||||||
|
|
||||||
|
- 헤딩 기준이 없다고 했는데 헤딩 기준 또한 IMU를 통해 얻을 수 있으므로, 직직시 편향 또한 보정이 가능한 것으로 보인다. 물론 IMU는 필터가 필요할 것이다.
|
||||||
|
|
||||||
|
- IMU는 MCU 내장 등 초 저가형은 사용하지 않을 것이며, HWT905 같은 IMU를 사용할 것이다.
|
||||||
@@ -0,0 +1,207 @@
|
|||||||
|
# 01. ZLAC8015D V4 하드웨어 스펙
|
||||||
|
|
||||||
|
출처: `resource/ZLAC8015D V4 Series Manual Version 1.03-20251111.pdf`
|
||||||
|
|
||||||
|
## 1.1 개요
|
||||||
|
|
||||||
|
* 제조사: Shenzhen Zhongling Technology (ZLTECH)
|
||||||
|
* 허브 서보 모터 전용 디지털 AC 서보 드라이버로, **1대가 좌/우 모터 2개(듀얼 채널)를 동시 구동**한다.
|
||||||
|
* RS485(Modbus RTU) 및 CANopen(CiA301/CiA402) 버스 통신을 내장.
|
||||||
|
* 위치/속도/토크 제어 모드를 지원.
|
||||||
|
* 용도: AGV, 배송 로봇, 서비스 로봇, 자동화 이송 장비 등 — 본 프로젝트에서는 실외 스키드 스티어
|
||||||
|
4WD 로봇의 좌우 휠 페어 구동에 사용된다.
|
||||||
|
|
||||||
|
## 1.2 전기 사양
|
||||||
|
|
||||||
|
| 파라미터 | 최소 | 표준 | 최대 | 단위 |
|
||||||
|
| :--- | :---: | :---: | :---: | :--- |
|
||||||
|
| 입력 전압 | 20 | 36 | 48 | VDC |
|
||||||
|
| 출력 전류 (피크) | 0 | 15 | 30 | A |
|
||||||
|
| 제어 신호 입력 전류 | 7 | 10 | 16 | mA |
|
||||||
|
| 과전압 보호 | - | 75 | - | VDC |
|
||||||
|
| 저전압 보호 | - | 16 | - | VDC |
|
||||||
|
| 입력 신호 전압 | - | 5 | - | VDC |
|
||||||
|
| 절연 저항 | 18 | 20 | - | MΩ |
|
||||||
|
|
||||||
|
> 채널당 정격전류 150(=15.0A), 최대전류 300(=30.0A)이 레지스터 `0x2033/0x2034`(좌),
|
||||||
|
> `0x2063/0x2064`(우)의 공장 기본값이며, 위 표의 15A/30A와 일치한다.
|
||||||
|
> 자세한 내용은 [05-error-troubleshooting.md](05-error-troubleshooting.md)의 전류 한계 절 참고.
|
||||||
|
|
||||||
|
## 1.3 환경 사양
|
||||||
|
|
||||||
|
| 항목 | 내용 |
|
||||||
|
| :--- | :--- |
|
||||||
|
| 냉각 방식 | 자연 냉각 또는 강제 냉각 |
|
||||||
|
| 사용 장소 | 먼지, 유분(oil mist), 부식성 가스 회피 |
|
||||||
|
| 동작 온도 | 0 ~ 50 ℃ |
|
||||||
|
| 최대 습도 | 90% RH (결로 없음) |
|
||||||
|
| 보관 온도 | -10 ~ 70 ℃ |
|
||||||
|
| 진동 | 10~55Hz / 0.15mm |
|
||||||
|
|
||||||
|
실외 로봇에 탑재되므로 방진/방수 인클로저 내부에 설치하고, 방열이 충분한 방향(narrow side,
|
||||||
|
방열판 면)으로 장착할 것을 매뉴얼이 권장한다. 주변 온도가 60℃를 넘는 밀폐/금속 분진 환경은
|
||||||
|
피해야 한다.
|
||||||
|
|
||||||
|
## 1.4 외형/설치 치수
|
||||||
|
|
||||||
|
* 크기: 150 × 97 × 31mm 내외 (방열판 폭 제외 케이스 기준, `resource/ZLAC8015D V4 Series.pdf` 도면 참고)
|
||||||
|
* M3 나사로 4모서리(와이드 사이드) 또는 양측면(네로우 사이드) 고정. 방열을 위해 네로우 사이드
|
||||||
|
설치 권장.
|
||||||
|
* 장착 시 진동/충격 보호 필요 (차체에 완충 마운트 권장).
|
||||||
|
|
||||||
|
## 1.5 커넥터 핀맵
|
||||||
|
|
||||||
|
### J1/J2 — 좌측 모터 전원 (5핀)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | DC | 전원 입력 24-48V |
|
||||||
|
| 2 | GND | 전원 GND |
|
||||||
|
| 3 | U | 좌모터 U상 |
|
||||||
|
| 4 | V | 좌모터 V상 |
|
||||||
|
| 5 | W | 좌모터 W상 |
|
||||||
|
|
||||||
|
### 우측 모터 전원 (5핀, 핀 순서 반대)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 5 | GND | 전원 GND |
|
||||||
|
| 4 | DC | 전원 입력 24-48V |
|
||||||
|
| 3 | W | 우모터 W상 |
|
||||||
|
| 2 | V | 우모터 V상 |
|
||||||
|
| 1 | U | 우모터 U상 |
|
||||||
|
|
||||||
|
전원은 좌/우 커넥터 중 한쪽 또는 양쪽 동시 입력이 가능하다(퀵스타트 가이드 확인).
|
||||||
|
|
||||||
|
### J2/J6 — 좌/우 모터 인크리멘탈 엔코더 & 홀 (12핀)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1-4 | iA+/iA-/iB+/iB- | 인크리멘탈 엔코더 A/B상 |
|
||||||
|
| 5-6 | RTC+/RTC- | 온도 센서 |
|
||||||
|
| 7-8 | V/W (Hall) | 홀 센서 |
|
||||||
|
| 9 | U (Hall) | 홀 센서 |
|
||||||
|
| 10 | GND | 전원 GND |
|
||||||
|
| 11 | VCC | 엔코더/홀 전원 출력 |
|
||||||
|
| 12 | GND | 전원 GND |
|
||||||
|
|
||||||
|
### J3 — 모터 제어 신호 포트 (8핀)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | BGND-L | 좌 브레이크 전원- |
|
||||||
|
| 2 | -BR-L | 좌 브레이크- |
|
||||||
|
| 3 | BDC-L | 좌 브레이크 전원+/브레이크+ |
|
||||||
|
| 4 | BGND-R | 우 브레이크 전원- |
|
||||||
|
| 5 | -BR-R | 우 브레이크- |
|
||||||
|
| 6 | BDC-R | 우 브레이크 전원+/브레이크+ |
|
||||||
|
| 7 | OUTPUT1 | 내부 풀업 5V 출력 (CAN/RS485로 기능 설정 가능) |
|
||||||
|
| 8 | OUTPUT2 | 내부 풀업 5V 출력 (CAN/RS485로 기능 설정 가능) |
|
||||||
|
|
||||||
|
전자식 브레이크는 레지스터 `0x201A`(B0, 좌)/`0x201B`(B1, 우)로 제어한다
|
||||||
|
(`0`=해제, `1`=잠금). 코드의 `MotorDriver::setBrakes()`가 이 두 레지스터에 동시에 쓴다.
|
||||||
|
|
||||||
|
### J4 — 모터 제어 신호 포트 (8핀)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | AOUT-R | 우모터 엔코더 A 출력 |
|
||||||
|
| 2 | BOUT-R | 우모터 엔코더 B 출력 |
|
||||||
|
| 3 | AOUT-L | 좌모터 엔코더 A 출력 |
|
||||||
|
| 4 | BOUT-L | 좌모터 엔코더 B 출력 |
|
||||||
|
| 5 | +5V | 엔코더 5V 출력 (<100mA) |
|
||||||
|
| 6 | GND | 엔코더 GND |
|
||||||
|
| 7 | INPUT1 | 프로그래머블 입력 (기본 비상정지 등으로 설정 가능) |
|
||||||
|
| 8 | INPUT2 | 프로그래머블 입력 |
|
||||||
|
|
||||||
|
INPUT1/INPUT2 펄스폭은 10ms 이상 유지해야 드라이버가 정상 인식한다. 입력 레벨은 기본 5V이며
|
||||||
|
12V 사용 시 1kΩ/0.5W, 24V 사용 시 2kΩ/0.5W 전류제한 저항을 외부에 추가해야 한다.
|
||||||
|
|
||||||
|
### J5 — 통신 포트 (5핀)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | CANH | CAN |
|
||||||
|
| 2 | CANL | CAN |
|
||||||
|
| 3 | SGND | 통신 GND |
|
||||||
|
| 4 | A | RS485 A |
|
||||||
|
| 5 | B | RS485 B |
|
||||||
|
|
||||||
|
**본 프로젝트는 이 중 SGND/A/B 3선을 사용해 RS485 Modbus RTU 통신을 한다.** 통신선은
|
||||||
|
차폐 트위스트페어 사용 및 접지 연결을 매뉴얼이 권장한다.
|
||||||
|
|
||||||
|
### eRS-R/eRS-L — 절대값 엔코더 인터페이스 (4핀, 미사용)
|
||||||
|
|
||||||
|
RS485 방식 외장 절대값 엔코더용 인터페이스. 본 로봇은 인크리멘탈 엔코더 기반 속도제어만
|
||||||
|
사용하므로 미배선.
|
||||||
|
|
||||||
|
### SW — DIP 스위치 (2핀)
|
||||||
|
|
||||||
|
| SW | 기능 |
|
||||||
|
| :-: | :--- |
|
||||||
|
| SW1 | CAN 종단저항 선택 |
|
||||||
|
| SW2 | RS485 종단저항 선택 |
|
||||||
|
|
||||||
|
데이지체인 마지막 드라이버에서만 종단저항을 ON 하는 것이 일반적인 RS485 배선 원칙이다.
|
||||||
|
|
||||||
|
### RES — 회생 저항 입력 (2핀)
|
||||||
|
|
||||||
|
| Pin | 표기 | 기능 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | R- | 브레이크 저항 |
|
||||||
|
| 2 | R+ | 브레이크 저항 |
|
||||||
|
|
||||||
|
100RPM 이상 속도 또는 급정지/비상정지 기능을 사용할 경우, 급감속 시 발생하는 역기전력으로부터
|
||||||
|
드라이버를 보호하기 위해 외부 회생 저항(권장 5~10Ω/100W) 연결을 권장한다. 본 로봇은 최고속도
|
||||||
|
0.4m/s(둘레 반경 131.5mm 기준 약 29RPM)로 저속 운용이라 리스크는 낮지만, 급제동(decel 150ms)
|
||||||
|
프로파일을 쓰므로 실외 장시간 운용 시 회생 저항 장착을 검토할 가치가 있다.
|
||||||
|
|
||||||
|
## 1.6 상태 LED
|
||||||
|
|
||||||
|
* **녹색 LED**: 전원 표시. 전원 인가 시 항상 점등.
|
||||||
|
* **적색 LED**: 알람 표시. 알람 발생 시 코드 횟수만큼 점멸 후 일시정지, 반복.
|
||||||
|
|
||||||
|
| 상태 | 점멸 횟수 | 원인 |
|
||||||
|
| :-: | :-: | :--- |
|
||||||
|
| Over-Voltage | 1 | 공급 전압이 최대 정격 초과 |
|
||||||
|
| Under-Voltage | 2 | 공급 전압이 최소 동작 전압 미만 |
|
||||||
|
| Over-Current | 3 | 상간 단락 등으로 모터 상전류 초과 |
|
||||||
|
| Over-Load | 4 | 상전류가 설정 과부하 전류 초과 |
|
||||||
|
| Current out-of-tolerance | 5 | (예약) |
|
||||||
|
| Position out-of-tolerance | 6 | 목표 위치와 출력 위치 오차 초과 |
|
||||||
|
| Speed out-of-tolerance | 7 | 목표 속도와 출력 속도 오차 초과 |
|
||||||
|
| Internal reference error | 8 | 드라이버 내부 오류 |
|
||||||
|
| Parameter reading error | 9 | EEPROM 파라미터 읽기 오류 |
|
||||||
|
| Hall fault | 10 | 홀 케이블 미접속 또는 신호 오류 |
|
||||||
|
| High motor temperature | 11 | 모터 과열 |
|
||||||
|
| Encoder error | 12 | 엔코더 케이블 헐거움/핀 순서 오류 |
|
||||||
|
| Driver overtemperature | 13 | 드라이버 과열 |
|
||||||
|
| Speed setting error | 14 | 지령 속도가 정격 속도 초과 |
|
||||||
|
| Mixed error | 15 | 2개 이상 알람 동시 발생 |
|
||||||
|
|
||||||
|
RS485로는 이 상태를 레지스터 `0x20A5`(좌)/`0x20A6`(우)에서 비트마스크로 읽을 수 있으며,
|
||||||
|
`xbox_motor_control.cpp`의 `decodeDriverError()`가 이를 한글 문자열로 변환한다. 상세는
|
||||||
|
[05-error-troubleshooting.md](05-error-troubleshooting.md) 참고.
|
||||||
|
|
||||||
|
## 1.7 배선 요약도 (본 프로젝트 기준)
|
||||||
|
|
||||||
|
```
|
||||||
|
┌─────────────────────────┐
|
||||||
|
배터리 24-48V ─┤ DC/GND (좌 또는 우 or 양쪽) │
|
||||||
|
│ │
|
||||||
|
좌측 허브모터 ─┤ U/V/W (좌) │ 드라이버 #1 (Station ID 1)
|
||||||
|
│ │ → 전방 좌/우 모터 구동
|
||||||
|
우측 허브모터 ─┤ U/V/W (우) │
|
||||||
|
│ │
|
||||||
|
좌 엔코더/홀 ─┤ J2 │
|
||||||
|
우 엔코더/홀 ─┤ J6 │
|
||||||
|
│ │
|
||||||
|
RS485 A/B/GND ┤ J5 ── (데이지체인) ──────┼──→ 드라이버 #2 (Station ID 2)
|
||||||
|
│ │ → 후방 좌/우 모터 구동
|
||||||
|
좌/우 브레이크 ─┤ J3 │
|
||||||
|
└─────────────────────────┘
|
||||||
|
│
|
||||||
|
/dev/ttyUSB0 (RS485-USB 컨버터)
|
||||||
|
│
|
||||||
|
제어 PC (xbox_motor_control_cpp)
|
||||||
|
```
|
||||||
@@ -0,0 +1,274 @@
|
|||||||
|
# 02. RS485 통신 프로토콜 & 레지스터 맵
|
||||||
|
|
||||||
|
출처: `resource/ZLAC8015D V4 Series RS485 Communication Version 1.06-20251111.pdf`,
|
||||||
|
`xbox_motor_control.cpp`
|
||||||
|
|
||||||
|
## 2.1 시리얼 포트 설정
|
||||||
|
|
||||||
|
| 항목 | 값 |
|
||||||
|
| :--- | :--- |
|
||||||
|
| 프로토콜 | Modbus RTU |
|
||||||
|
| 보드레이트 | 115200 bps (기본값, 코드에서 사용) — 9600/19200/38400/57600/128000 선택 가능 |
|
||||||
|
| 데이터 비트 | 8 |
|
||||||
|
| 패리티 | None |
|
||||||
|
| 스톱 비트 | 1 |
|
||||||
|
| 드라이버 주소 범위 | 1 ~ 127 (본 프로젝트: 드라이버#1=1, 드라이버#2=2) |
|
||||||
|
|
||||||
|
`xbox_motor_control.cpp`의 `SerialPort::openPort()`가 termios로 위 설정(8N1, RAW 모드,
|
||||||
|
VTIME=1 → 0.1s read timeout)을 그대로 구성한다.
|
||||||
|
|
||||||
|
## 2.2 Modbus 프레임 포맷
|
||||||
|
|
||||||
|
```
|
||||||
|
[슬레이브 주소 1B][기능코드 1B][데이터 N B][CRC16 2B (Low,High 순)]
|
||||||
|
```
|
||||||
|
|
||||||
|
CRC는 표준 Modbus CRC16(다항식 0xA001, 초기값 0xFFFF) — `calcCRC16()`이 이를 그대로 구현.
|
||||||
|
|
||||||
|
### 지원 기능 코드
|
||||||
|
|
||||||
|
| 기능 | 코드 | 에러 응답 코드 |
|
||||||
|
| :--- | :-: | :-: |
|
||||||
|
| 다중 레지스터 읽기 | `0x03` | `0x83` |
|
||||||
|
| 단일 레지스터 쓰기 | `0x06` | `0x86` |
|
||||||
|
| 다중 레지스터 쓰기 | `0x10` | `0x90` |
|
||||||
|
|
||||||
|
| 에러 코드 | 이름 | 의미 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| `0x01` | Illegal function code | 지원하지 않는 기능 코드 |
|
||||||
|
| `0x02` | Illegal data address | 잘못된 레지스터 주소 |
|
||||||
|
| `0x03` | Illegal data value | 잘못된 데이터 값 |
|
||||||
|
|
||||||
|
### 코드 구현 매핑
|
||||||
|
|
||||||
|
| 클래스 함수 | 기능 코드 | 용도 |
|
||||||
|
| :--- | :-: | :--- |
|
||||||
|
| `SerialPort::readRegs()` | 0x03 | 피드백/상태/전류한계 조회 |
|
||||||
|
| `SerialPort::writeReg()` | 0x06 | 제어워드/모드/브레이크 등 단일 레지스터 쓰기 |
|
||||||
|
| `SerialPort::writeRegs()` | 0x10 | 가감속시간, 목표속도(L/R 동시) 쓰기 |
|
||||||
|
|
||||||
|
응답 검증은 CRC16 재계산 + 슬레이브 주소 일치 여부로 이루어지며(`writeReg`/`writeRegs`/`readRegs`
|
||||||
|
모두 동일), 브로드캐스트(`slave==0`)일 때는 응답을 기다리지 않는다(`--bcast` 옵션).
|
||||||
|
|
||||||
|
읽기 타임아웃은 25ms(100Hz=10ms 주기 루프 안에서 여유를 둔 값), 쓰기 응답 타임아웃은 50ms로
|
||||||
|
설정되어 있다.
|
||||||
|
|
||||||
|
## 2.3 코드가 실제로 사용하는 레지스터 (초기화/제어/피드백 경로)
|
||||||
|
|
||||||
|
| 주소 | 이름 | 방향 | 코드 사용처 |
|
||||||
|
| :-- | :--- | :-: | :--- |
|
||||||
|
| `0x200D` | Control mode | W | `initDriver()`: `3` = 속도 제어 모드 고정 |
|
||||||
|
| `0x200E` | Control word | W | `initDriver()`: `0x0006` 알람 클리어 → `0x0007` Disable → (가감속 설정 후) `0x0008` Enable |
|
||||||
|
| `0x2080` / `0x2081` | 가속시간 좌/우 | W | `initDriver(acl_ms)`가 `writeRegs(0x2080,{acl,acl})`로 2레지스터 동시 기록 (기본 150ms) |
|
||||||
|
| `0x2082` / `0x2083` | 감속시간 좌/우 | W | `initDriver(dcl_ms)`가 `writeRegs(0x2082,{dcl,dcl})`로 동시 기록 (기본 150ms) |
|
||||||
|
| `0x2088` / `0x2089` | 목표속도 좌/우 (RPM, I16, ±3000) | W | `setRPMs()`: 매 틱 `writeRegs(0x2088,{l,r})`로 좌/우 동시 지령 |
|
||||||
|
| `0x201A` | 출력단자 B0 (좌 브레이크) | W | `setBrakes()`: `0`=해제, `1`=잠금 |
|
||||||
|
| `0x201B` | 출력단자 B1 (우 브레이크) | W | `setBrakes()`: 좌와 동일 값 동시 기록 |
|
||||||
|
| `0x2033` / `0x2034` | 좌 정격전류 / 좌 최대전류 (0.1A) | R/W | `readCurrentLimits()`로 조회, `setMaxCurrent()`로 `0x2034`만 기록 |
|
||||||
|
| `0x2063` / `0x2064` | 우 정격전류 / 우 최대전류 (0.1A) | R/W | 좌와 동일 패턴 (우 채널) |
|
||||||
|
| `0x20A5` ~ `0x20AE` | 에러코드~토크 (아래 표 참고) | R | `readFeedback()`가 **10레지스터 연속 읽기**(0x03, count=10)로 한 번에 확보 |
|
||||||
|
|
||||||
|
### `readFeedback()`의 10레지스터 연속 읽기 상세 (`0x20A5` 시작, count=10)
|
||||||
|
|
||||||
|
| offset | 주소 | 이름 | 코드 내 변수 | 스케일 |
|
||||||
|
| :-: | :-- | :--- | :--- | :--- |
|
||||||
|
| 0 | `0x20A5` | 에러코드(좌) | `err_l` | 비트마스크 (그대로) |
|
||||||
|
| 1 | `0x20A6` | 에러코드(우) | `err_r` | 비트마스크 (그대로) |
|
||||||
|
| 2 | `0x20A7` | 실제위치 상위16비트(좌) | `val_l` 상위 | `int32_t` 합성 → `l_tick` |
|
||||||
|
| 3 | `0x20A8` | 실제위치 하위16비트(좌) | `val_l` 하위 | 〃 |
|
||||||
|
| 4 | `0x20A9` | 실제위치 상위16비트(우) | `val_r` 상위 | `int32_t` 합성 → `r_tick` |
|
||||||
|
| 5 | `0x20AA` | 실제위치 하위16비트(우) | `val_r` 하위 | 〃 |
|
||||||
|
| 6 | `0x20AB` | 실제속도(좌), 단위 0.1r/min | `vl` | `× 0.1f` → `l_fb` (RPM) |
|
||||||
|
| 7 | `0x20AC` | 실제속도(우), 단위 0.1r/min | `vr` | `× 0.1f` → `r_fb` (RPM) |
|
||||||
|
| 8 | `0x20AD` | 실제토크/전류(좌), 단위 0.1A | `tl` | `× 0.1f` → `l_torque_a` (A) |
|
||||||
|
| 9 | `0x20AE` | 실제토크/전류(우), 단위 0.1A | `tr` | `× 0.1f` → `r_torque_a` (A) |
|
||||||
|
|
||||||
|
이 한 번의 통신으로 에러코드·엔코더 위치(주행거리 계산용)·속도 피드백·전류(부하 진단용)를
|
||||||
|
모두 얻기 때문에, 100Hz 루프에서 드라이버 1대당 통신 1회(읽기)+1회(쓰기)만 필요하다.
|
||||||
|
2대 드라이버 × 2회 = 틱당 최대 4회 왕복, `comm_ms`로 HUD에 실측 소요시간을 표시한다.
|
||||||
|
|
||||||
|
> 위치(`0x20A7`~`0x20AA`)는 엔코더 라인수 1024 기준 4체배(quadrature)로 회전당
|
||||||
|
> `1024 × 4 = 4096`이 아니라, README 기준 실측 `16,384 ticks/rev`를 사용한다
|
||||||
|
> (ZLLG80ASM250-**4096**-B 모터명의 4096은 엔코더 라인수이며, 4체배 카운팅 시
|
||||||
|
> `4096 × 4 = 16384`가 된다. `0x2030`/`0x2060` "Encoder line" 파라미터 공장 기본값 1024와는
|
||||||
|
> 별개로, 실제 장착된 ZLLG80ASM250-4096-B 모터는 라인수 4096 사양이므로 드라이버의
|
||||||
|
> Encoder line 파라미터가 모터 사양(4096)에 맞춰 설정되어 있어야 `meters_per_tick` 계산이
|
||||||
|
> 정확하다. 드라이버 실측정격전류가 15A인 것도 이 모터의 정격과 일치.)
|
||||||
|
|
||||||
|
## 2.4 에러코드 (`0x20A5` / `0x20A6`)
|
||||||
|
|
||||||
|
| 값 | 의미 | 코드 내 한글 표기 |
|
||||||
|
| :-- | :--- | :--- |
|
||||||
|
| `0x0000` | 정상 (No error) | (없음) |
|
||||||
|
| `0x0001` | 과전압 (Over voltage) | 과전압 |
|
||||||
|
| `0x0002` | 저전압 (Under voltage) | 저전압 |
|
||||||
|
| `0x0004` | 과전류 (Over current) | 과전류 |
|
||||||
|
| `0x0008` | 과부하 (Over load) | 과부하 |
|
||||||
|
| `0x0010` | 전류 이상 (예약) | 전류이상(예약) |
|
||||||
|
| `0x0020` | 엔코더 오차 (Encoder out of tolerance) | 엔코더오차 |
|
||||||
|
| `0x0040` | 속도 이상 (예약) | 속도이상(예약) |
|
||||||
|
| `0x0080` | 기준전압 오류 (Reference voltage error) | 기준전압오류 |
|
||||||
|
| `0x0100` | EEPROM 오류 | EEPROM오류 |
|
||||||
|
| `0x0200` | 홀센서 오류 | 홀센서오류 |
|
||||||
|
| `0x0400` | 모터 과열 | 모터과열 |
|
||||||
|
| `0x0800` | 엔코더 오류 | 엔코더오류 |
|
||||||
|
| `0x2000` | 속도설정 오류 (지령속도가 정격속도 초과) | 속도설정오류 |
|
||||||
|
|
||||||
|
여러 비트가 동시에 서면 코드가 `"+"`로 이어붙여 표시한다 (`decodeDriverError()`).
|
||||||
|
드라이버 알람 상세 진단과 대응은 [05-error-troubleshooting.md](05-error-troubleshooting.md) 참고.
|
||||||
|
|
||||||
|
## 2.5 제어워드(`0x200E`)와 초기화 시퀀스
|
||||||
|
|
||||||
|
| 값 | 의미 |
|
||||||
|
| :-- | :--- |
|
||||||
|
| `0x05` | 비상 정지 (Emergency stop) |
|
||||||
|
| `0x06` | 알람 클리어 (Clear fault) |
|
||||||
|
| `0x07` | 정지 (Stop / Disable) |
|
||||||
|
| `0x08` | 활성화 (Enable) |
|
||||||
|
| `0x10` | 동기 시작 (Start Synchronous, 포지션 모드 전용) |
|
||||||
|
| `0x11` | 좌측 단독 시작 (Start Left, 포지션 모드) |
|
||||||
|
| `0x12` | 우측 단독 시작 (Start Right, 포지션 모드) |
|
||||||
|
|
||||||
|
`MotorDriver::initDriver()`의 실제 시퀀스 (각 단계 사이 30ms 대기):
|
||||||
|
|
||||||
|
1. `0x200E` ← `0x0006` (알람 클리어)
|
||||||
|
2. `0x200E` ← `0x0007` (Disable)
|
||||||
|
3. `0x200D` ← `3` (속도 제어 모드)
|
||||||
|
4. `0x2080`/`0x2081` ← 가속시간 (좌우 동시, 기본 150ms)
|
||||||
|
5. `0x2082`/`0x2083` ← 감속시간 (좌우 동시, 기본 150ms)
|
||||||
|
6. `0x200E` ← `0x0008` (Enable)
|
||||||
|
7. `setBrakes(true)` — 초기 상태는 브레이크 잠금
|
||||||
|
|
||||||
|
이 프로젝트는 속도 제어 모드(Profile Velocity Mode)만 사용하며, 포지션/토크 모드 관련 레지스터
|
||||||
|
(`0x208A`~`0x208F`, `0x2090`/`0x2091` 등)는 사용하지 않는다. 참고용으로 §2.7에 전체 표를 남긴다.
|
||||||
|
|
||||||
|
## 2.6 전류 한계 레지스터
|
||||||
|
|
||||||
|
| 주소 | 이름 | 단위 | 기본값 | 범위 |
|
||||||
|
| :-- | :--- | :-: | :-: | :--- |
|
||||||
|
| `0x2033` | 좌 정격전류 | 0.1A | 150 (15.0A) | 0-150 |
|
||||||
|
| `0x2034` | 좌 최대전류 | 0.1A | 300 (30.0A) | 0-300 |
|
||||||
|
| `0x2063` | 우 정격전류 | 0.1A | 150 (15.0A) | 0-150 |
|
||||||
|
| `0x2064` | 우 최대전류 | 0.1A | 300 (30.0A) | 0-300 |
|
||||||
|
|
||||||
|
`readCurrentLimits()`가 시작 시 1회 조회해 콘솔에 출력하고, `--max_current_a` 인자
|
||||||
|
(기본 20.0A)로 최대전류를 4바퀴 동일하게 낮춘다. 정격전류(150=15A)는 쓰지 않고 최대전류만
|
||||||
|
변경한다 — 근거와 튜닝 배경은 [05-error-troubleshooting.md](05-error-troubleshooting.md) 참고.
|
||||||
|
|
||||||
|
## 2.7 전체 레지스터 주소록 (Address Directory, 매뉴얼 §4 원문 기준)
|
||||||
|
|
||||||
|
### 공통 상수 (Left/Right 공통)
|
||||||
|
|
||||||
|
| 주소 | 이름 | 접근 | 기본값 | 설명 |
|
||||||
|
| :-- | :--- | :-: | :-: | :--- |
|
||||||
|
| `2000h` | Communication offline time | RW/S | 0 | ms, 0-32000. 시간 내 통신 없으면 정지 |
|
||||||
|
| `2001h` | RS485 Node ID | RW/S | 1 | 1-127 |
|
||||||
|
| `2002h` | RS485 Baud Rate | RW/S | 2 | 1:128000 2:115200 3:57600 4:38400 5:19200 6:9600 |
|
||||||
|
| `2003h` | Input signal status | RO | 0 | bit0-1: X0-X1 입력 레벨 |
|
||||||
|
| `2004h` | Output signal status | RO | 0 | bit0-1: Y0-Y1 출력 레벨 |
|
||||||
|
| `2005h` | Clear feedback position | RW | 0 | 0 무효, 1 좌, 2 우, 3 좌우 (미저장) |
|
||||||
|
| `2006h` | Reset absolute zero point | RW | 0 | 0 무효, 1 좌, 2 우, 3 좌우 (미저장) |
|
||||||
|
| `2007h` | Shaft state after power on | RW/S | 0 | 0 Enable 안 됨/락 안 됨, 1 Enable 안 됨/락 |
|
||||||
|
| `2008h` | Maximum motor speed | RW/S | 1000 | r/min, 1-1000 |
|
||||||
|
| `2009h` | Register parameter settings | RW | 0 | 0 무효, 1 공장 초기화 |
|
||||||
|
| `200Ah` | CAN Node ID | RW/S | 1 | 1-127 |
|
||||||
|
| `200Bh` | CAN Baud rate | RW/S | 1 | 0:1000 1:500 2:250 3:125 4:100 Kbit/s |
|
||||||
|
| `200Ch` | Parking mode | RW/S | 0 | 0 Close, 1 Open |
|
||||||
|
| `200Dh` | Control mode | RW/S | 0 | 0 undefined, 1 위치(상대), 2 위치(절대), 3 속도, 4 토크 |
|
||||||
|
| `200Eh` | Control word | RW | 0 | §2.5 참고 |
|
||||||
|
| `200Fh` | Sync/Async control status | RW/S | 0 | 0 동기, 1 비동기 |
|
||||||
|
| `2010h` | RW 레지스터 EEPROM 저장 여부 | RW | 0 | 0 무효, 1 저장 |
|
||||||
|
| `2011h` | Quick stop control | RW/S | 5 | 5 정지, 6 감속시간 적용 급정지, 7 감속시간 미적용 급정지 |
|
||||||
|
| `2012h` | Close operation control | RW/S | 1 | 0 무효, 1 정상정지 |
|
||||||
|
| `2013h` | Disable control | RW/S | 1 | 0 무효, 1 정지(switch on 상태로) |
|
||||||
|
| `2014h` | Halt control | RW/S | 1 | 1 정지, 2 감속 급정지, 3 무감속 급정지 |
|
||||||
|
| `2016h` | Input effective level | RW/S | 0 | bit0 X0, bit1 X1 (0 기본 High, 1 반전 Low) |
|
||||||
|
| `2017h` | Input X0 function | RW/S | 9 | 0 none, 1-8 NC, 9 비상정지 |
|
||||||
|
| `2018h` | Input X1 function | RW/S | 0 | 〃 |
|
||||||
|
| `2019h` | Output effective level | RW/S | 0 | bit0 Y0, bit1 Y1, bit2 B0, bit3 B1 |
|
||||||
|
| `201Ah` | **출력 B0 (좌 브레이크)** | RW/S | 0 | 0 Open, 1 Close — **코드 사용** |
|
||||||
|
| `201Bh` | **출력 B1 (우 브레이크)** | RW/S | 0 | 0 Open, 1 Close — **코드 사용** |
|
||||||
|
| `201Ch` | Output Y0 function | RW/S | 0 | 0 undefined, 1 알람신호, 2 상태신호, 3 목표위치도달(예약) |
|
||||||
|
| `201Dh` | Output Y1 function | RW/S | 0 | 〃 |
|
||||||
|
| `201Eh` | 드라이버 온도 보호 임계값 | RW/S | 800 | 0.1℃, 0-1200 |
|
||||||
|
| `201Fh` | Alarm PWM 처리 방식 | RW/S | 0 | 0 close, 1 open |
|
||||||
|
| `2020h` | Overload 처리 방식 | RW/S | 0 | 0 close, 1 open |
|
||||||
|
| `2021h` | I/O 비상정지 처리 모드 | RW/S | 0 | 0 축고정(우선I), 1 댐핑(우선II), 2 축해제(우선III); low8=INPUT1 high8=INPUT2 |
|
||||||
|
| `2022h` | Given speed resolution | RW/S | 1 | 1:1RPM ... A:0.1RPM |
|
||||||
|
| `2023h` | Velocity overshoot | RW/S | 1 | 0 close, 1 open |
|
||||||
|
| `2024h` | Regen resistance value | RW/S | 50 | 0.1Ω, 0-1000 |
|
||||||
|
| `2025h` | Regen resistance power | RW/S | 100 | W, 0-1000 |
|
||||||
|
| `2026h` | Regen opening voltage | RW/S | 650 | 0.1V, 360-750 |
|
||||||
|
| `2027h` | Regen close voltage | RW/S | 600 | 0.1V, 310-700 |
|
||||||
|
| `2028h` | Regen function control | RW/S | 1 | 0 close, 1 open |
|
||||||
|
| `2029h` | Default running direction | RW/S | 0 | 0 CW, 1 CCW |
|
||||||
|
|
||||||
|
### 좌모터 파라미터 (`2030h`~`204Ah`) / 우모터 파라미터 (`2060h`~`207Ah`, 좌+0x30 오프셋)
|
||||||
|
|
||||||
|
| 좌 주소 | 우 주소 | 이름 | 접근 | 기본값 | 설명 |
|
||||||
|
| :-- | :-- | :--- | :-: | :-: | :--- |
|
||||||
|
| `2030h` | `2060h` | Encoder line | RW/S | 1024 | 0-4096 |
|
||||||
|
| `2031h` | `2061h` | Hall offset angle | RW/S | 0 | 1°, -360~360 |
|
||||||
|
| `2032h` | `2062h` | Overload factor | RW/S | 200 | %, 0-300 |
|
||||||
|
| `2033h` | `2063h` | **정격전류** | RW/S | 150 | 0.1A, 0-150 — **코드 사용** |
|
||||||
|
| `2034h` | `2064h` | **최대전류** | RW/S | 300 | 0.1A, 0-300 — **코드 사용** |
|
||||||
|
| `2035h` | `2065h` | Overload protection time | RW/S | 300 | 10ms, 0-6553 |
|
||||||
|
| `2036h` | `2066h` | Position following error threshold | RW/S | 409 | 10counts, 1-6553 |
|
||||||
|
| `2037h` | `2067h` | Velocity smoothing factor | RW/S | 1000 | 0-30000 |
|
||||||
|
| `2038h` | `2068h` | Current Loop Kp | RW/S | 600 | 0-30000 |
|
||||||
|
| `2039h` | `2069h` | Current Loop Ki | RW/S | 300 | 0-30000 |
|
||||||
|
| `203Ah` | `206Ah` | Feedforward output smoothing | RW/S | 100 | 0-30000 |
|
||||||
|
| `203Bh` | `206Bh` | Torque output smoothing | RW/S | 100 | 0-30000 |
|
||||||
|
| `203Ch` | `206Ch` | Velocity Loop Kp | RW/S | 500 | 0-30000 |
|
||||||
|
| `203Dh` | `206Dh` | Velocity Loop Ki | RW/S | 100 | 0-30000 |
|
||||||
|
| `203Eh` | `206Eh` | Velocity Loop Kf | RW/S | 500 | 0-30000 |
|
||||||
|
| `203Fh` | `206Fh` | Position Loop Kp | RW/S | 100 | 0-30000 |
|
||||||
|
| `2040h` | `2070h` | Position Loop Kf | RW/S | 50 | 0-30000 |
|
||||||
|
| `2043h` | `2073h` | Initial velocity (속도모드) | RW/S | 1 | r/min, 1-250 |
|
||||||
|
| `2044h` | `2074h` | Initial velocity (위치모드) | RW/S | 1 | r/min, 1-250 |
|
||||||
|
| `2045h` | `2075h` | Motor poles | RW/S | 15 | 4-64 |
|
||||||
|
| `2046h` | `2076h` | Over temperature threshold | RW/S | 800 | 0.1℃, 0-1200 |
|
||||||
|
| `2047h`-`204Ah` | `2077h`-`207Ah` | Velocity observer coefficients 1-4 | RW/S | 1000/750/350/1000 | 0-30000 |
|
||||||
|
|
||||||
|
### 제어 파라미터 (`2080h`~`2091h`)
|
||||||
|
|
||||||
|
| 주소 | 이름 | 접근 | 기본값 | 설명 |
|
||||||
|
| :-- | :--- | :-: | :-: | :--- |
|
||||||
|
| `2080h` | **S자 가속시간(좌)** | RW/S | 500ms | 0-32767ms — **코드 사용 (150ms)** |
|
||||||
|
| `2081h` | **S자 가속시간(우)** | RW/S | 500ms | 0-32767ms — **코드 사용 (150ms)** |
|
||||||
|
| `2082h` | **S자 감속시간(좌)** | RW/S | 500ms | 0-32767ms — **코드 사용 (150ms)** |
|
||||||
|
| `2083h` | **S자 감속시간(우)** | RW/S | 500ms | 0-32767ms — **코드 사용 (150ms)** |
|
||||||
|
| `2084h` | 급정지 감속시간(좌) | RW/S | 10ms | 0-32767ms |
|
||||||
|
| `2085h` | 급정지 감속시간(우) | RW/S | 10ms | 0-32767ms |
|
||||||
|
| `2086h` | 토크 슬로프(좌) | RW/S | 300ms | mA/s(1/1000초 단위) |
|
||||||
|
| `2087h` | 토크 슬로프(우) | RW/S | 300ms | 〃 |
|
||||||
|
| `2088h` | **목표속도(좌)** | RW | 0 | I16, r/min, ±3000 — **코드 사용, 매 틱 기록** |
|
||||||
|
| `2089h` | **목표속도(우)** | RW | 0 | I16, r/min, ±3000 — **코드 사용, 매 틱 기록** |
|
||||||
|
| `208Ah`/`208Bh` | 목표위치 상/하위(좌) | RW | 0 | 포지션 모드 전용 (미사용) |
|
||||||
|
| `208Ch`/`208Dh` | 목표위치 상/하위(우) | RW | 0 | 포지션 모드 전용 (미사용) |
|
||||||
|
| `208Eh`/`208Fh` | 최대속도(좌/우, 포지션모드) | RW/S | 120 r/min | 포지션 모드 전용 (미사용) |
|
||||||
|
| `2090h`/`2091h` | 목표토크(좌/우) | RW | 0 | mA, ±30000 — 토크 모드 전용 (미사용) |
|
||||||
|
|
||||||
|
### 읽기 전용 상태 (`20A0h`~`20B0h`)
|
||||||
|
|
||||||
|
| 주소 | 이름 | 단위/범위 | 코드 사용 여부 |
|
||||||
|
| :-- | :--- | :--- | :--- |
|
||||||
|
| `20A0h` | Software version | - | 미사용 |
|
||||||
|
| `20A1h` | Bus voltage | 0.01V | 미사용 (배터리 전압 모니터링에 활용 가능) |
|
||||||
|
| `20A2h` | Status word | L:bit7-6, R:bit15-14 → 00 해제/40 축고정/80 비상정지/C0 알람; bit0(L)/bit8(R) 0정지 1구동 | 미사용 |
|
||||||
|
| `20A3h` | Hall input state | 0-7 (0,7이면 홀 오류), high8=좌 low8=우 | 미사용 |
|
||||||
|
| `20A4h` | Motor temperature | 1℃, -55~120, high8=좌 low8=우 | 미사용 (과열 사전감지에 활용 가능) |
|
||||||
|
| `20A5h` | **에러코드(좌)** | 비트마스크 (§2.4) | **코드 사용** |
|
||||||
|
| `20A6h` | **에러코드(우)** | 비트마스크 (§2.4) | **코드 사용** |
|
||||||
|
| `20A7h`/`20A8h` | **실제위치 상/하위(좌)** | counts, I16×2 | **코드 사용** → `l_tick` |
|
||||||
|
| `20A9h`/`20AAh` | **실제위치 상/하위(우)** | counts, I16×2 | **코드 사용** → `r_tick` |
|
||||||
|
| `20ABh` | **실제속도(좌)** | 0.1r/min | **코드 사용** → `l_fb` |
|
||||||
|
| `20ACh` | **실제속도(우)** | 0.1r/min | **코드 사용** → `r_fb` |
|
||||||
|
| `20ADh` | **실제토크/전류(좌)** | 0.1A, ±300 | **코드 사용** → `l_torque_a` |
|
||||||
|
| `20AEh` | **실제토크/전류(우)** | 0.1A, ±300 | **코드 사용** → `r_torque_a` |
|
||||||
|
| `20AFh` | Software connected status | `01` | 미사용 |
|
||||||
|
| `20B0h` | Driver temperature | 0.1℃, -550~1200 | 미사용 (과열 사전감지에 활용 가능) |
|
||||||
|
|
||||||
|
> **미사용이지만 유용한 레지스터**: `20A1h`(버스 전압), `20A4h`/`20B0h`(모터/드라이버 온도)는
|
||||||
|
> 현재 코드가 읽지 않는다. 배터리 저전압 경보나 열 관리 기능을 추가하려면 `readFeedback()`의
|
||||||
|
> 연속 읽기 범위를 확장하거나 별도 저빈도 폴링을 추가하는 것을 검토할 수 있다.
|
||||||
@@ -0,0 +1,110 @@
|
|||||||
|
# 03. 로봇/드라이버 시스템 구성
|
||||||
|
|
||||||
|
출처: `README.md`, `xbox_motor_control.cpp` (main 함수 상단 파라미터), 실측값
|
||||||
|
|
||||||
|
## 3.1 로봇 물리 제원
|
||||||
|
|
||||||
|
| 항목 | 값 | 비고 |
|
||||||
|
| :--- | :-- | :--- |
|
||||||
|
| 구동 방식 | 스키드 스티어(Skid-Steer) 4WD | 좌/우 각 2륜씩 기계적으로 독립 구동, 조향 없음 |
|
||||||
|
| 전폭 (Track Width, W) | 576.0 mm | 좌우 바퀴 중심 간 거리 |
|
||||||
|
| 전장 축간거리 (Wheelbase, L) | 368.4 mm | 전후 바퀴 중심 간 거리 |
|
||||||
|
| 휠 유효 반경 (R) | 131.517 mm | 버니어 실측값, 10.35인치 타이어 |
|
||||||
|
| 전면 범퍼 오프셋 | 45.0 mm | 휠 축 → 로봇 최전방 범퍼 |
|
||||||
|
| 로봇 전폭(차체) | 410 mm | 라이다 회피 폭 계산 기준 |
|
||||||
|
| 로봇 전장(차체) | 631 mm | |
|
||||||
|
| 로봇 전고 | 286 mm | |
|
||||||
|
|
||||||
|
## 3.2 모터 & 드라이버 매핑
|
||||||
|
|
||||||
|
| 위치 | 모터 | 소속 드라이버 | Station ID | 시리얼 포트 | 드라이버 채널 |
|
||||||
|
| :--- | :--- | :-: | :-: | :--- | :-: |
|
||||||
|
| 전방 좌 (FL) | ZLLG80ASM250-4096-B V2.12 | 드라이버 #1 (전방) | 1 | `/dev/ttyUSB0` | Left |
|
||||||
|
| 전방 우 (FR) | ZLLG80ASM250-4096-B V2.12 | 드라이버 #1 (전방) | 1 | `/dev/ttyUSB0` | Right |
|
||||||
|
| 후방 좌 (RL) | ZLLG80ASM250-4096-B V2.12 | 드라이버 #2 (후방) | 2 | `/dev/ttyUSB0` (동일 포트, 데이지체인) 또는 `--port2` | Left |
|
||||||
|
| 후방 우 (RR) | ZLLG80ASM250-4096-B V2.12 | 드라이버 #2 (후방) | 2 | 〃 | Right |
|
||||||
|
|
||||||
|
* 드라이버 2대는 **동일 RS485 버스(같은 USB-RS485 포트)를 공유**하는 것이 기본 동작이며,
|
||||||
|
Modbus 슬레이브 주소(1, 2)로 구분한다. `--port2`를 지정하면 별도의 물리 포트를 사용하도록
|
||||||
|
전환 가능(`sp2_ptr`가 `&sp1` 대신 `&sp2`를 가리키게 됨).
|
||||||
|
* `--bcast`/`--broadcast` 옵션 사용 시 두 드라이버 모두 주소 `0`(브로드캐스트)으로 동시에
|
||||||
|
명령을 받는다 — 이 경우 개별 피드백 조회/전류 조회는 생략된다(`driver_rear_ptr = nullptr`
|
||||||
|
로 유지, 앞뒤 드라이버가 응답을 구분해 회신할 수 없기 때문).
|
||||||
|
* 좌/우 부호가 반대인 이유: `MotorDriver::setRPMs(l_rpm, r_rpm)`에 넘기는 지령에서
|
||||||
|
`target_fr = -v_r * rpm_per_ms`, `target_rr = -v_r * rpm_per_ms`처럼 **우측 바퀴는 항상
|
||||||
|
부호 반전**해서 넘긴다. 좌/우 모터가 로봇 상에서 물리적으로 반대 방향으로 장착되어 있어
|
||||||
|
(양쪽 모터가 "전진"으로 회전해야 하는 U/V/W 위상이 거울 대칭) 이렇게 보정하지 않으면
|
||||||
|
우측 바퀴가 반대로 회전한다.
|
||||||
|
|
||||||
|
## 3.3 엔코더 해상도
|
||||||
|
|
||||||
|
* 모터 1회전당 **16,384 ticks** (4체배 카운팅 기준)
|
||||||
|
* `meters_per_tick = (2π × wheel_radius) / 16384`
|
||||||
|
* 주행거리(Odometry)는 4륜 엔코더 델타의 **중앙값(4개 중 중간 2개 평균)**으로 계산해,
|
||||||
|
웅덩이 등으로 한 바퀴가 들떠 헛도는 경우의 이상치를 자동으로 배제한다 (`dist_axle`).
|
||||||
|
`dist_bumper = dist_axle + bumper_offset`(전진 시)으로 범퍼 기준 거리도 함께 산출.
|
||||||
|
|
||||||
|
## 3.4 스키드 스티어 운동학
|
||||||
|
|
||||||
|
### 유효 선회 반경 계수
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
effective_w = sqrt(track_width² + wheelbase²) // 대각선 유효 폭
|
||||||
|
k_skid = (track_width + wheelbase² / track_width) / 2
|
||||||
|
```
|
||||||
|
|
||||||
|
`k_skid`는 순수 사각 스키드 스티어(회전축이 로봇 중심)보다 실제 마찰 중심을 반영해 보정한
|
||||||
|
계수로, 직진+커브 선회 시 좌/우 바퀴 속도 차를 계산하는 데 사용된다.
|
||||||
|
|
||||||
|
### 모드 1 — 직진/커브 선회 (`is_spin_turn == false`)
|
||||||
|
|
||||||
|
좌측 스틱 전후(ly)로 전진 속도 `v_x = ly × max_v`를 만들고, 좌측 스틱 좌우(lx)로 곡선 조향을
|
||||||
|
만든다. 저속에서 커브 반경이 스로틀과 무관하게 일정하도록, 각속도 `omega`는 **현재 전진 속도
|
||||||
|
`v_x`에 비례**하도록 계산된다(상수 `max_v`가 아님 — 저속에서 안쪽 바퀴가 역회전해버리는 버그를
|
||||||
|
막기 위한 수정):
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
omega += -lx * (abs(v_x) / (effective_w / 2))
|
||||||
|
v_l = v_x - omega * k_skid
|
||||||
|
v_r = v_x + omega * k_skid
|
||||||
|
target_fl = target_rl = v_l * rpm_per_ms
|
||||||
|
target_fr = target_rr = -v_r * rpm_per_ms // 우측 부호 반전
|
||||||
|
```
|
||||||
|
|
||||||
|
### 모드 2 — 제자리 회전 (`is_spin_turn == true`, ly==0 && rx!=0)
|
||||||
|
|
||||||
|
우측 스틱 좌우(rx)만으로 제자리 선회한다. 순수 제자리 회전(ICR이 로봇 중심)은 고마찰 노면에서
|
||||||
|
정지마찰이 급격히 파괴돼 바퀴가 긁히므로, **미세한 전진 성분(`spin_arc_bias`, 기본 12%)**을
|
||||||
|
섞어 ICR을 로봇 중심에서 살짝 벗어나게 해 스크럽 마찰을 줄인다:
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
omega = -rx * (max_spin_v / (effective_w / 2))
|
||||||
|
v_bias = abs(rx) * max_spin_v * spin_arc_bias
|
||||||
|
v_l = v_bias - omega * (effective_w / 2)
|
||||||
|
v_r = v_bias + omega * (effective_w / 2)
|
||||||
|
```
|
||||||
|
|
||||||
|
제자리 회전 진입 조건은 `ly == 0.0f`의 **완전 일치 비교**다. `applyDeadzone()`이 데드존
|
||||||
|
이하 입력을 정확히 `0.0f`로 반환하기 때문에 안전하며, 부동소수점 근사 오차로 인한 오탐을
|
||||||
|
막기 위해 별도 임계값을 두지 않고 정확히 이 특성에 의존한다.
|
||||||
|
|
||||||
|
### 라이다 자동 회피 조향 (측면 회피 시, 조이스틱 좌우 입력이 없을 때)
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
v_l = v_x - omega * k_skid
|
||||||
|
v_r = v_x + omega * k_skid
|
||||||
|
```
|
||||||
|
(제자리 회전이 아니면서 `lx==rx==0`이고 `omega!=0`인 경우 — [04-software-control.md](04-software-control.md)
|
||||||
|
의 라이다 회피 절 참고)
|
||||||
|
|
||||||
|
## 3.5 속도 → 드라이버 RPM 지령 변환
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
rpm_per_ms = 60 / (2π × wheel_radius) // m/s → RPM 변환 계수
|
||||||
|
```
|
||||||
|
|
||||||
|
계산된 `target_*`(RPM)는 곧바로 드라이버에 보내지 않고, 저크 제한(Jerk-Limited) S-curve
|
||||||
|
프로파일(`jerkLimitedStep()`)을 거쳐 `cmd_*`로 서서히 수렴한 뒤 `setRPMs()`로 전달된다.
|
||||||
|
소프트웨어 측 가감속/저크 프로파일은 드라이버 자체의 가감속 레지스터(`0x2080`~`0x2083`,
|
||||||
|
150ms)보다 상위 레벨에서 한 번 더 부드럽게 다듬는 이중 구조다. 상세는
|
||||||
|
[04-software-control.md](04-software-control.md) 참고.
|
||||||
@@ -0,0 +1,194 @@
|
|||||||
|
# 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축 범위 |
|
||||||
@@ -0,0 +1,91 @@
|
|||||||
|
# 05. 알람 진단 & 트러블슈팅
|
||||||
|
|
||||||
|
출처: `xbox_motor_control.cpp` 주석/구현, `resource/ZLAC8015D V4 Series RS485 Communication*.pdf`,
|
||||||
|
`resource/ZLAC8015D V4 Series Manual*.pdf`
|
||||||
|
|
||||||
|
## 5.1 드라이버 알람(에러코드) 대응표
|
||||||
|
|
||||||
|
콘솔에 `[!!! 드라이버 알람 발생 !!!]`이 출력되면 `0x20A5`(좌)/`0x20A6`(우) 값을 근거로
|
||||||
|
`decodeDriverError()`가 원인을 표시한다. HUD의 `[!!알람!!]` 태그도 `fault_active` 동안 계속
|
||||||
|
표시된다.
|
||||||
|
|
||||||
|
| 비트 | 한글 표기 | 가능한 원인 | 권장 대응 |
|
||||||
|
| :-- | :--- | :--- | :--- |
|
||||||
|
| `0x0001` 과전압 | 과전압 | 배터리 전압이 75V 근접/초과, 급감속 시 회생 전류 유입 | 배터리 전압 확인, 회생 저항(`RES` 단자) 장착 검토, `2011h` 급정지 설정을 감속시간 포함(6) 방식으로 |
|
||||||
|
| `0x0002` 저전압 | 저전압 | 배터리 방전, 배선 전압강하, 순간 대전류로 전압 새그 | 배터리 충전 상태·배선 굵기 확인, `--max_current_a`로 최대전류를 낮춰 순간 전압강하 완화 |
|
||||||
|
| `0x0004` 과전류 | 과전류 | 상간 단락, 급격한 부하 변화, 배선 손상 | 배선/커넥터 점검, 모터 상간 저항 측정. 지속 시 드라이버 교체 검토 |
|
||||||
|
| `0x0008` 과부하 | 과부하 | 지형 저항/걸림 상태가 `2035h`(과부하 보호시간, 기본 300×10ms=3s) 이상 지속 | §5.3 전류 한계 튜닝 참고. 걸림(Stall) HUD 표시와 함께 발생하면 지형/장애물 확인 |
|
||||||
|
| `0x0020` 엔코더오차 | 엔코더오차 | 위치 폐루프 추종 오차가 `2036h`(기본 4090 counts) 초과 | 급격한 외력(충돌, 헛도는 지면)으로 순간 발생 가능. 반복 시 엔코더 배선 확인 |
|
||||||
|
| `0x0080` 기준전압오류 | 기준전압오류 | 드라이버 내부 기준전압 회로 이상 | 드라이버 하드웨어 이상 — 제조사 문의 |
|
||||||
|
| `0x0100` EEPROM오류 | EEPROM오류 | 파라미터 저장영역 읽기 실패 | `2009h=1`로 공장 초기화 후 필요한 파라미터(전류한계, 가감속 등) 재설정 |
|
||||||
|
| `0x0200` 홀센서오류 | 홀센서오류 | 홀 케이블 미접속/단선, 핀 순서 오류 | J2/J6 커넥터 및 홀 배선 점검 |
|
||||||
|
| `0x0400` 모터과열 | 모터과열 | 모터 온도가 임계치 초과(과부하 지속 주행 등) | 잠시 정지해 냉각, 반복 시 `--max_current_a` 하향 또는 부하 조건 재검토 |
|
||||||
|
| `0x0800` 엔코더오류 | 엔코더오류 | 엔코더 케이블 헐거움/핀 순서 오류(매뉴얼 명시) | J2/J6 커넥터 재체결, 케이블 단선 여부 확인 |
|
||||||
|
| `0x2000` 속도설정오류 | 속도설정오류 | 지령속도가 정격 속도(`2008h`, 기본 1000rpm) 초과 | 코드의 `setRPMs()` 클램프(±3000)가 드라이버 정격보다 커서 이론상 발생 가능 — `2008h` 값과 실제 사용 RPM 범위 확인 |
|
||||||
|
|
||||||
|
> `0x0010`(전류이상), `0x0040`(속도이상)은 매뉴얼상 예약(Reserved) 비트로 정상 동작에서는
|
||||||
|
> 발생하지 않는다.
|
||||||
|
|
||||||
|
**공통 대응 원칙**: 알람 발생 시 드라이버가 명령을 무시하는 잠금 상태가 될 수 있어 소프트웨어
|
||||||
|
재시작만으로는 풀리지 않는 경우가 많다. RS485로 `0x200E ← 0x0006`(알람 클리어) 후
|
||||||
|
`0x200E ← 0x0008`(Enable) 순으로 재초기화가 필요하며, `initDriver()`가 프로그램 재시작 시
|
||||||
|
이 시퀀스를 자동 수행한다. 그래도 반복되면 전원 재투입(Power Cycle)을 시도한다.
|
||||||
|
|
||||||
|
## 5.2 휠 이상(들뜸/걸림) 진단 — 알람이 아닌 소프트웨어 레벨 진단
|
||||||
|
|
||||||
|
드라이버 알람과 별개로, 애플리케이션 레벨에서 4륜 각각의 상태를 실시간 분류한다
|
||||||
|
(원리는 [04-software-control.md](04-software-control.md) §4.8 참고). HUD `WHL` 표시:
|
||||||
|
|
||||||
|
| 표시 | 의미 | 조치 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| `O` | 정상(Ok) | - |
|
||||||
|
| `A` | 들뜸(Airborne) — 동료 바퀴 대비 전류가 비정상적으로 낮음 (무부하 헛돎) | 자동으로 해당 바퀴 지령을 0으로 낮춤. 지속되면 지형(웅덩이, 턱) 확인 |
|
||||||
|
| `S` | 걸림(Stall) — 지령 대비 실제속도가 크게 못 따라가며 고전류 지속 | 장애물 끼임, 지형 저항 확인. 반복 시 `0x0008` 과부하 알람으로 이어질 수 있음 |
|
||||||
|
|
||||||
|
이 진단은 어디까지나 소프트웨어 추정치이며, 드라이버 알람(§5.1)과는 독립적으로 동작한다.
|
||||||
|
`--log`로 CSV를 남기면 `stat_*`/`amp_*` 컬럼으로 사후 분석이 가능하다.
|
||||||
|
|
||||||
|
## 5.3 최대전류(`0x2034`/`0x2064`) 튜닝 배경
|
||||||
|
|
||||||
|
코드 기본값은 `--max_current_a 20.0`(공장 기본 최대전류 30A보다 낮춤). 코드 주석에 남겨진
|
||||||
|
실측 배경:
|
||||||
|
|
||||||
|
> 실측 결과 정격 15A / 최대 30A(공장 기본값) 그대로 사용 시, 걸림/과부하 상황에서 드라이버가
|
||||||
|
> 30A까지 전류를 밀어붙이며 3초(과부하 보호시간 `2035h` 기본값)씩 버티다가 과부하 알람이
|
||||||
|
> 발생하는 문제가 있었다. 정격(15A) 위로 5A 정도의 여유는 남기되, 상한을 20A로 낮춰 과도한
|
||||||
|
> 전류로 장시간 버티는 상황 자체를 줄인 것.
|
||||||
|
|
||||||
|
* `--max_current_a`에 음수를 주면 드라이버에 저장된 기존/공장 설정을 그대로 둔다(미변경).
|
||||||
|
* 정격전류(`0x2033`/`0x2063`)는 코드가 변경하지 않는다 — 최대전류만 낮춘다.
|
||||||
|
* 실외 험지 주행에서 순간 고부하가 잦다면 이 값을 상황에 맞게 재조정할 것. 너무 낮추면
|
||||||
|
정상 등판/가속 상황에서도 전류 제한에 걸려 토크 부족이 발생할 수 있다.
|
||||||
|
* 시작 시 콘솔에 출력되는 "전방/후방 드라이버 전류설정" 로그로 **실제 드라이버에 저장된 값**을
|
||||||
|
항상 확인할 수 있다(문서 기본값이 아니라 하드웨어에 실제로 쓰여있는 값).
|
||||||
|
|
||||||
|
## 5.4 RS485 통신 트러블슈팅
|
||||||
|
|
||||||
|
| 증상 | 가능한 원인 | 확인/조치 |
|
||||||
|
| :--- | :--- | :--- |
|
||||||
|
| 시작 시 "포트 열기 실패" | 잘못된 `/dev/ttyUSB*` 경로, 권한 부족 | `ls /dev/ttyUSB*`로 실제 장치명 확인, `dialout` 그룹 권한 확인 |
|
||||||
|
| "전류설정 조회 실패" 경고 | 배선 불량, 종단저항 미설정, 슬레이브 주소 불일치 | J5 커넥터 A/B/SGND 배선, 데이지체인 마지막 드라이버의 SW2 종단저항, `--id1`/`--id2`가 실제 드라이버 주소와 일치하는지 확인 |
|
||||||
|
| 명령은 가는데 응답이 자주 누락(HUD `comm_ms` 급등, 피드백 값이 갱신 안 됨) | 100Hz 루프 대비 응답 지연/노이즈, USB latency_timer 미적용 | `set_low_latency.sh` 적용 여부 확인(`cat /sys/bus/usb-serial/devices/<dev>/latency_timer`가 1이어야 함), 차폐 트위스트페어 케이블 및 접지 확인 |
|
||||||
|
| 알람은 없는데 바퀴가 반대로 회전 | 좌/우 모터 배선 반대 장착 오인, `2029h` 기본 회전방향 설정 문제 | [03-system-architecture.md](03-system-architecture.md) §3.2의 우측 부호 반전 로직 확인. 하드웨어적으로 U/V/W가 뒤바뀐 경우 `2029h`(Default running direction)로도 보정 가능 |
|
||||||
|
| CRC 불일치로 통신 실패 반환 (`readRegs`/`writeReg`가 `false`) | 전기 노이즈, 접지 불량, 데이지체인 배선 길이 과다 | 코드는 CRC16을 재계산해 손상된 응답을 자동으로 폐기하므로 오탐 지령은 방지되지만, 값 자체가 갱신되지 않아 HUD가 멈춘 것처럼 보일 수 있다. 배선 점검 필요 |
|
||||||
|
|
||||||
|
> 코드는 통신 실패 시 **재시도(retry)를 하지 않는다** — 실패한 틱은 그냥 다음 100Hz 틱으로
|
||||||
|
> 넘어간다. 이는 저지연(latency)을 우선한 설계 선택이며, 지속적인 통신 불량은 알람이 아니라
|
||||||
|
> "피드백이 갱신되지 않음"으로만 나타날 수 있으므로 HUD의 `comm_ms` 값과 위치/전류 피드백이
|
||||||
|
> 실제로 변하는지 함께 관찰해야 한다.
|
||||||
|
|
||||||
|
## 5.5 운용 전 체크리스트
|
||||||
|
|
||||||
|
1. 배선: 좌/우 U/V/W, 엔코더/홀(J2·J6), RS485 A/B/SGND(J5), 브레이크(J3) 재확인.
|
||||||
|
2. RS485 종단저항: 데이지체인 마지막 드라이버에서만 SW2 ON.
|
||||||
|
3. `run_4wd.sh` 실행 후 콘솔에서 "전방/후방 드라이버 전류설정" 값이 기대값(정격15A 근방,
|
||||||
|
최대 20~30A)인지 확인.
|
||||||
|
4. 알람 없이 브레이크가 해제/잠금되는지(휠 수동 회전 저항으로 체감) 확인.
|
||||||
|
5. 저속으로 A버튼(트립리셋) → 직진 → 정지 → 후진 테스트로 좌/우 회전 방향이 의도대로인지
|
||||||
|
확인.
|
||||||
|
6. 라이다 연결 시 HUD에 `LIDAR:OFF`가 아닌 실측 거리(`F:x.xxm|L:...|R:...`)가 표시되는지 확인.
|
||||||
|
7. 실외 주행 전 `--log`로 CSV 기록을 켜서 최초 주행 구간을 남겨두면 사후 튜닝(§5.3, §4.11의
|
||||||
|
`--stall_*`/`--airborne_*` 인자)에 유용하다.
|
||||||
@@ -0,0 +1,162 @@
|
|||||||
|
# 06. IMU 도입을 통한 제어 고도화 — 조사 및 계획 (실행 전 검토용)
|
||||||
|
|
||||||
|
> **이 문서는 계획안이다. 코드는 아직 수정하지 않았다.** 아래 내용을 검토하고 방향에 동의하면
|
||||||
|
> 그때 `xbox_motor_control.cpp`에 반영을 시작한다.
|
||||||
|
>
|
||||||
|
> **갱신 이력**: 이 문서에 대한 1차 Q&A가 [`QnA/q1.md`](../QnA/q1.md) / [`QnA/a1.md`](../QnA/a1.md)에
|
||||||
|
> 정리되어 있다. 개루프/폐루프 계층 구조에 대한 실무 관행, 온라인 ICR 추정 가능성, 헤딩 홀드에
|
||||||
|
> 필요한 필터링 수준, IMU 하드웨어(HWT905) 확정 내용이 아래 §6.4, §6.7에 반영되어 있다.
|
||||||
|
|
||||||
|
출처: `resource/husky.md`(Clearpath Husky User Manual 링크), Clearpath `robot_localization`
|
||||||
|
연동 자료, Yi, J. et al., *"Kinematic Modeling and Analysis of Skid-Steered Mobile Robots With
|
||||||
|
Applications to Low-Cost Inertial-Measurement-Unit-Based Motion Estimation,"* IEEE Transactions
|
||||||
|
on Robotics, 25(5), 2009 (이하 "Yi 2009"), Pentzer, J. et al., *"Model-based Prediction of
|
||||||
|
Skid-steer Robot Kinematics Using Online Estimation of Track Instantaneous Centers of Rotation,"*
|
||||||
|
Journal of Field Robotics, 2014 (이하 "Pentzer 2014"), 그리고 본 저장소
|
||||||
|
`xbox_motor_control.cpp` 현재 구현.
|
||||||
|
|
||||||
|
## 6.1 왜 이 조사를 하는가
|
||||||
|
|
||||||
|
Husky(Clearpath Robotics)는 본 로봇과 동일한 **스키드 스티어 4WD 실외 플랫폼**이다. Husky의
|
||||||
|
표준 소프트웨어 스택은 휠 오도메트리만 쓰지 않고, `robot_localization`의
|
||||||
|
`ekf_localization_node`로 **휠 오도메트리 + IMU**를 융합해 `/odometry/filtered`를 만들고, 이를
|
||||||
|
헤딩 유지·자율주행의 기준으로 삼는다. 이 구조와 그 이론적 근거(Yi 2009)를 우리 시스템과
|
||||||
|
비교하면, 현재 우리 제어가 왜 "투박"한지, 그리고 IMU 추가가 실제로 어떤 문제를 해결하는지가
|
||||||
|
명확해진다.
|
||||||
|
|
||||||
|
## 6.2 현재 구현의 한계 (코드 근거)
|
||||||
|
|
||||||
|
| # | 한계 | 코드 근거 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | **완전 개루프(open-loop) 텔레옵.** 조이스틱 입력 → 킨매틱스 수식 → 저크제한 램프 → RPM 지령까지, 로봇이 실제로 그 지령대로 움직였는지 확인하는 피드백이 전무하다. | `main()` 루프 전체 — `target_*` 계산에 어떤 실측 모션 피드백도 관여하지 않음 |
|
||||||
|
| 2 | **헤딩/자세(x, y, θ) 추정 자체가 없다.** 주행거리(trip meter)는 4륜 엔코더 델타의 중앙값이라는 스칼라 값 하나뿐이고, 회전 방향/누적 헤딩은 어디에도 저장되지 않는다. | `dist_axle` 계산부 (엔코더 tick → 거리만 산출, 각속도 적분 없음) |
|
||||||
|
| 3 | **고정 트랙폭 공식이 ICR을 로봇 중심으로 가정.** `k_skid = (track_width + wheelbase²/track_width) / 2`는 Yi 2009가 지적하는 "단순화된 근사"이며, 실제 좌/우/로봇 ICR이 중심에서 벗어나는 지면 마찰 효과를 반영하지 않는다. | `k_skid`, `effective_w` 계산부 |
|
||||||
|
| 4 | **매직넘버로 근사 오차를 손땜빵.** `spin_arc_bias=0.12`(제자리 회전 시 스크럽 마찰 완화용 임의 전진 비율), `spin_jerk_rate=220`(제자리 회전만 저크를 더 낮춤) 등은 모두 "ICR이 실제로는 중심이 아니다"라는 동일 현상을 실측 없이 경험적으로 조정한 값들이다. | 코드 상단 파라미터 선언부 주석 |
|
||||||
|
| 5 | **슬립 진단이 전류만으로 이루어지는 간접 추정.** `classifyStall`/`classifyAirPair`는 지령 대비 실제 RPM과 전류만 보고 개별 바퀴 상태를 추정한다. 전류는 부하·경사·배터리 전압 새그에도 영향받으므로, "로봇 전체가 실제로 회전/가속하고 있는가"라는 직접적 진단은 불가능하다. | `classifyStall()`, `classifyAirPair()` |
|
||||||
|
| 6 | **직진 시 편향(drift) 자동 보정 수단이 없다.** 좌/우 지면 마찰이 다르면 동일 RPM을 줘도 로봇이 서서히 한쪽으로 틀어지는데, 이를 감지할 헤딩 기준 자체가 없어 순전히 운전자가 조이스틱으로 수동 보정해야 한다. | 없음 (기능 자체 부재) |
|
||||||
|
|
||||||
|
## 6.3 Yi 2009 논문이 제시하는 제어/추정 방식 (요약)
|
||||||
|
|
||||||
|
1. **3-ICR 운동학 모델**: 좌측 휠(트랙), 우측 휠(트랙), 로봇 몸체 각각에 별도의 순간회전중심
|
||||||
|
(ICR_l, ICR_r, ICR_b) 좌표를 부여한다. 기존 "트랙폭 하나로 결정되는" 모델을 일반화한 것으로,
|
||||||
|
`(v_l, v_r) → (v_x, ω)` 관계가 이 3개 ICR 좌표를 파라미터로 하는 선형식이 된다.
|
||||||
|
2. **오프라인/온라인 캘리브레이션**: ICR 좌표는 고정 상수로 가정하지 않고, 실측 모션(외부
|
||||||
|
기준 또는 IMU)에 대해 최소자승으로 피팅하거나 EKF 상태의 일부로 추정한다. Pentzer 2014는
|
||||||
|
이를 한 발 더 발전시켜 **속도·지형에 따라 ICR이 실시간으로 변한다고 보고 온라인으로 계속
|
||||||
|
추정**하는 방식을 제안한다.
|
||||||
|
3. **IMU-EKF 융합**: 자이로 요레이트는 슬립과 무관하게 로봇의 실제 회전각속도를 직접 측정한다.
|
||||||
|
이를 ICR 모델의 휠-기반 예측치와 EKF로 결합하면, 저가 스트랩다운 IMU + 휠 인코더만으로도
|
||||||
|
위치/헤딩 추정 정확도가 크게 향상됨을 실험으로 보인다.
|
||||||
|
4. **부가 효과 — 잔차 기반 슬립 검출**: 휠 기반 ω 예측치와 자이로 실측치의 차이(잔차) 자체가
|
||||||
|
정량적 슬립 지표가 된다. 이는 §6.2-5에서 지적한 "전류만으로 하는 간접 추정"보다 훨씬 직접적인
|
||||||
|
신호다.
|
||||||
|
|
||||||
|
## 6.4 단계별 개선 계획 (Phase 1 → 5, 각 단계는 독립적으로 가치가 있고 이전 단계에 얹는 구조)
|
||||||
|
|
||||||
|
> 설계 원칙: 각 Phase는 **먼저 관측/로깅으로 검증**한 뒤에만 제어 루프에 개입하도록 순서를
|
||||||
|
> 잡았다. 검증 없이 곧바로 폐루프 보정을 넣으면, IMU 노이즈나 마운트 문제를 슬립으로 오판해
|
||||||
|
> 오히려 제어가 나빠질 위험이 있기 때문이다(§6.6 리스크 참고).
|
||||||
|
|
||||||
|
### Phase 0 — IMU(HWT905) 정식 매뉴얼 확보 및 레지스터 맵 문서화 (선행 작업)
|
||||||
|
|
||||||
|
* IMU 하드웨어는 **WitMotion HWT905-RS485**로 확정 (근거: `QnA/a1.md` §Q4 — 9축 AHRS 융합
|
||||||
|
출력, RS485/Modbus 프로토콜 지원, IP67, 최대 200Hz).
|
||||||
|
* RS485+Modbus라는 점에서 `xbox_motor_control.cpp`의 `SerialPort`(Modbus RTU, CRC16,
|
||||||
|
`readReg`/`writeReg`/`readRegs`)를 상당 부분 재사용 가능할 것으로 예상되나, **정확한
|
||||||
|
레지스터 주소(각속도/각도/가속도/지자기, 출력레이트, 슬레이브 주소 설정)는 아직 확정하지
|
||||||
|
못했다** — 확보한 정식 PDF가 이미지 스캔본이라 텍스트 추출이 안 됐기 때문.
|
||||||
|
* 실행 순서: (1) 텍스트 검색 가능한 정식 매뉴얼을 `resource/`에 추가 → (2) ZLAC8015D 문서와
|
||||||
|
동일한 정밀도로 `doc/07-imu-hwt905-spec.md`(가칭) 작성 → (3) 그 다음에야 Phase 1 코드 작업
|
||||||
|
착수.
|
||||||
|
* **버스 분리 여부 결정 필요**: ZLAC8015D 드라이버 2대가 쓰는 RS485 버스에 IMU를 슬레이브로
|
||||||
|
얹을지, 별도 USB-RS485 포트로 분리할지는 실측(3개 슬레이브를 한 버스에 얹었을 때 100Hz
|
||||||
|
루프의 `comm_ms` 여유)으로 결정한다(§6.7 참고).
|
||||||
|
|
||||||
|
### Phase 1 — IMU 연동 + 순수 관측 (제어 루프 미개입)
|
||||||
|
|
||||||
|
* Phase 0에서 확정한 레지스터로 HWT905에서 요레이트(Z축 자이로) 최소, 가능하면 roll/pitch도
|
||||||
|
함께 읽는다. 장착 위치는 차체 무게중심에 근접하고 진동이 적은 곳.
|
||||||
|
* 별도 스레드에서 100Hz 이상으로 폴링해 값을 전역/원자적으로 공유(기존
|
||||||
|
`LidarObstacleDetector`가 이미 별도 스레드+뮤텍스 패턴을 쓰고 있으므로 동일 패턴 재사용
|
||||||
|
가능).
|
||||||
|
* **제어 로직은 건드리지 않는다.** 요레이트 값을 HUD 한 줄에 추가 표시하고, `--log` CSV에
|
||||||
|
`imu_gyro_z` 컬럼만 추가.
|
||||||
|
* 목적: 실제 하드웨어 노이즈 특성, 진동 영향, 드리프트/바이어스 크기를 먼저 눈으로 확인.
|
||||||
|
HWT905는 온보드 AHRS 필터를 내장하므로(`QnA/a1.md` §Q3), 애플리케이션 레벨에서 별도
|
||||||
|
저역통과/상보필터를 얼마나 더 추가해야 하는지는 이 단계의 실측으로 판단한다.
|
||||||
|
|
||||||
|
### Phase 2 — 헤딩(θ) 적분 + x,y 오도메트리 노출
|
||||||
|
|
||||||
|
* 자이로 요레이트를 단순 적분해 헤딩 θ를 추정하고, 기존 `dist_axle`(휠 기반 선속도 크기)과
|
||||||
|
결합해 x, y 위치도 함께 적분.
|
||||||
|
* 휠 기반 ω(현재 `k_skid` 공식으로부터 나오는 값)와 IMU 기반 ω를 **나란히** HUD/CSV에 출력해
|
||||||
|
두 추정치가 실제로 얼마나 차이 나는지 정량적으로 확인. 이 차이가 바로 §6.3-4의 슬립 잔차다.
|
||||||
|
* 아직 제어에는 반영하지 않음 — Phase 1과 마찬가지로 "계측 단계".
|
||||||
|
|
||||||
|
### Phase 3 — 직진 헤딩 홀드 (첫 폐루프 개입)
|
||||||
|
|
||||||
|
* 조향 입력(`lx`, `rx`)이 0이고 전진/후진 중일 때, θ를 "락(lock)"하고 IMU 실측 요레이트가
|
||||||
|
0에서 벗어나면 작은 PI 트림을 좌/우 `cmd_*`에 더해 편향을 보정.
|
||||||
|
* 개입 폭에 상한을 두어(예: 최대 트림 RPM 제한) 사용자의 의도적 조향을 절대 압도하지 않게 함.
|
||||||
|
* 이 단계부터 "운전자가 매번 손으로 직진 편향을 보정하는" 현재의 투박함이 실질적으로 개선됨.
|
||||||
|
|
||||||
|
### Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (기존 전류 진단 보완)
|
||||||
|
|
||||||
|
* 지령 ω 대비 IMU 실측 ω의 오차가 임계값을 넘으면 "전체 슬립"(예: 젖은 잔디, 빙판, 4륜 동시
|
||||||
|
헛돎)으로 판정 — 기존 `classifyStall`/`classifyAirPair`(개별 휠·전류 기반)와는 독립적인
|
||||||
|
신호이므로 서로 교차검증하는 2중 진단 체계가 됨.
|
||||||
|
* 전체 슬립 판정 시 속도를 일시적으로 낮추는 등, 기존 라이다 회피의 `speed_scale` 메커니즘과
|
||||||
|
유사한 패턴으로 개입 가능.
|
||||||
|
|
||||||
|
### Phase 5 — ICR 실측 캘리브레이션으로 매직넘버 제거 (정적 → 온라인 확장)
|
||||||
|
|
||||||
|
* **5a. 정적 캘리브레이션 (먼저 검증)**: 다양한 속도·회전반경 조합으로 주행하며 지령 대비 IMU
|
||||||
|
실측 ω를 기록, 최소자승으로 유효 트랙폭/ICR 오프셋을 피팅. `k_skid`, `effective_w`, 그리고
|
||||||
|
경험적으로 넣은 `spin_arc_bias=0.12` 같은 값을 이 실측 피팅값으로 교체하거나 근거를 마련.
|
||||||
|
* **5b. 온라인 ICR 추정 (선택적 확장, `QnA/a1.md` §Q2)**: ICR은 실제로는 경사·무게중심·지형
|
||||||
|
마찰·트레드 마모에 따라 계속 변하는 값이므로(Pentzer 2014), 5a의 정적 상수 대신
|
||||||
|
`effective_ICR_gain = 실측_요레이트(IMU) / 지령_차동속도`를 매 틱 갱신하는 방식으로 확장할
|
||||||
|
수 있다. 구현/검증 부담이 커지므로 5a가 실외에서 충분히 검증된 뒤에만 착수.
|
||||||
|
* 커밋 이력(`19fb70d Fix curve-steering wheel-reversal at partial throttle`,
|
||||||
|
`ae3f92f Improve outdoor traction control`)에서 반복적으로 나타난 "저속/부분 스로틀에서
|
||||||
|
커브가 이상하게 도는" 류의 버그들은 상당수가 이 근사식의 부정확성에서 기인했을 가능성이
|
||||||
|
높다 — 근본 원인 제거 시도.
|
||||||
|
|
||||||
|
### Phase 6 (장기, 선택) — 자율주행 확장 가능성
|
||||||
|
|
||||||
|
* Phase 2에서 만든 x,y,θ 추정을 Livox Mid-360S(이미 장착, 현재는 반응형 회피에만 사용)와
|
||||||
|
결합하면 SLAM/웨이포인트 주행으로 자연스럽게 확장 가능. 이번 계획의 범위 밖이지만, IMU
|
||||||
|
도입이 갖는 장기적 가치로 기록해 둔다.
|
||||||
|
|
||||||
|
## 6.5 Husky 실제 소프트웨어 스택과의 대응 관계
|
||||||
|
|
||||||
|
| Husky (`robot_localization` 기반) | 본 계획의 대응 Phase |
|
||||||
|
| :--- | :--- |
|
||||||
|
| 휠 인코더 오도메트리 (`/husky_velocity_controller/odom`) | 이미 존재 (`readFeedback()` 기반 `l_fb`/`r_fb`, `l_tick`/`r_tick`) |
|
||||||
|
| IMU (`/imu/data`) | Phase 1 |
|
||||||
|
| `ekf_localization_node` (휠+IMU 융합 → `/odometry/filtered`) | Phase 2, 장기적으로 정식 EKF로 고도화 가능 |
|
||||||
|
| 헤딩 기반 주행 안정성 | Phase 3 |
|
||||||
|
| Nav Stack 기반 자율주행 | Phase 6 (장기) |
|
||||||
|
|
||||||
|
## 6.6 리스크 / 트레이드오프
|
||||||
|
|
||||||
|
* **진동 노이즈**: 실외 오프로드 특성상 차체 진동이 크다. 저역통과/상보필터 없이 자이로 값을
|
||||||
|
바로 쓰면 오히려 노이즈를 슬립으로 오판할 수 있다 — Phase 1의 "순수 관측" 단계를 반드시
|
||||||
|
거쳐 실제 노이즈 특성을 먼저 파악해야 한다.
|
||||||
|
* **마운트 위치**: 무게중심에서 멀거나 진동이 심한 곳에 달면 가속도계 값이 왜곡된다. 요레이트는
|
||||||
|
위치에 덜 민감하지만 완전히 자유롭지는 않다.
|
||||||
|
* **추가 통신/스레드 부하**: 100Hz 제어 루프에 영향을 주지 않도록 IMU 폴링은 별도 스레드로
|
||||||
|
분리(라이다와 동일 패턴)하고, 메인 루프는 원자적 읽기만 수행해야 한다.
|
||||||
|
* **개발/검증 비용**: Phase 3 이후의 폐루프 개입은 실외 실주행 테스트로만 튜닝 가능하다 —
|
||||||
|
단계별로 나눠 각 단계를 충분히 검증한 뒤 다음 단계로 넘어가는 접근이 필요하다.
|
||||||
|
|
||||||
|
## 6.7 결정이 필요한 사항
|
||||||
|
|
||||||
|
| # | 항목 | 상태 |
|
||||||
|
| :-: | :--- | :--- |
|
||||||
|
| 1 | **목표 범위**: Phase 3(헤딩 홀드)까지만 목표로 할지, Phase 6(자율주행 확장)까지 염두에 두고 설계할지 — Phase 2 오도메트리 구조(단순 적분 vs 정식 EKF)에 영향 | 미결정 |
|
||||||
|
| 2 | **IMU 하드웨어** | **결정됨 — WitMotion HWT905-RS485** (`QnA/a1.md` §Q4) |
|
||||||
|
| 3 | **RS485 버스 분리 여부**: IMU를 ZLAC8015D와 같은 버스에 얹을지, 별도 포트로 분리할지 | 미결정 — Phase 1 실측 후 결정 |
|
||||||
|
| 4 | **시작 지점**: Phase 0(매뉴얼 확보/문서화)부터 순차 진행할지, 특정 Phase만 우선 원하는지 | 미결정 |
|
||||||
|
|
||||||
|
이 문서에 대한 피드백을 반영해 계획을 확정한 뒤 코드 작업을 시작한다.
|
||||||
@@ -0,0 +1,39 @@
|
|||||||
|
# 드라이버 문서화 (ZLAC8015D V4)
|
||||||
|
|
||||||
|
이 문서 세트는 `resource/` 폴더의 ZLTECH ZLAC8015D V4 시리즈 서보 드라이버 매뉴얼을 분석하고,
|
||||||
|
본 저장소의 실제 제어 코드(`xbox_motor_control.cpp`)가 그 드라이버를 어떻게 사용하고 있는지
|
||||||
|
정리한 것이다. 매뉴얼 원문(영문 PDF)과 실제 구현 사이의 매핑을 명확히 하는 데 초점을 둔다.
|
||||||
|
|
||||||
|
## 로봇/드라이버 구성 요약
|
||||||
|
|
||||||
|
* **로봇 형식**: 실외 스키드 스티어(Skid-Steer) 4WD 로봇
|
||||||
|
* **모터**: ZLLG80ASM250-4096-B V2.12 허브 서보 모터 × 4 (전좌/전우/후좌/후우)
|
||||||
|
* **드라이버**: ZLAC8015D V4 (듀얼 채널 서보 드라이버) × 2대
|
||||||
|
* 드라이버 1대당 좌/우 모터 2개를 동시에 구동 (Left/Right 채널)
|
||||||
|
* 드라이버 #1 (Station ID 1) → 전방 좌/우 모터
|
||||||
|
* 드라이버 #2 (Station ID 2) → 후방 좌/우 모터
|
||||||
|
* **통신**: RS485 Modbus RTU, 115200bps 8N1, 두 드라이버가 `/dev/ttyUSB0` 한 포트에 데이지체인
|
||||||
|
* **제어 PC 소프트웨어**: `xbox_motor_control.cpp` (C++17, 100Hz 제어 루프, SDL2 Xbox 컨트롤러 입력,
|
||||||
|
Livox Mid-360S 라이다 장애물 회피 통합)
|
||||||
|
|
||||||
|
## 문서 목차
|
||||||
|
|
||||||
|
| 문서 | 내용 |
|
||||||
|
| :--- | :--- |
|
||||||
|
| [01-hardware-spec.md](01-hardware-spec.md) | ZLAC8015D 하드웨어 스펙, 커넥터 핀맵, 배선, 설치, LED 진단 |
|
||||||
|
| [02-communication-protocol.md](02-communication-protocol.md) | RS485 Modbus RTU 프로토콜과 실제 코드가 사용하는 레지스터 맵 |
|
||||||
|
| [03-system-architecture.md](03-system-architecture.md) | 로봇 물리 제원, 2드라이버×4모터 매핑, 스키드 스티어 운동학 |
|
||||||
|
| [04-software-control.md](04-software-control.md) | `xbox_motor_control.cpp` 제어 루프/클래스 구조 분석 |
|
||||||
|
| [05-error-troubleshooting.md](05-error-troubleshooting.md) | 알람 에러코드 표, 휠 이상(들뜸/걸림) 진단, 전류 한계 튜닝, 트러블슈팅 |
|
||||||
|
| [06-imu-integration-plan.md](06-imu-integration-plan.md) | **(계획안, 미구현)** IMU 도입을 통한 제어 고도화 조사 및 단계별 계획 — Husky/논문 비교 |
|
||||||
|
|
||||||
|
06번 문서에 대한 Q&A 기록은 [`../QnA/`](../QnA/) 폴더에 별도로 누적된다 (예: [`q1.md`](../QnA/q1.md) / [`a1.md`](../QnA/a1.md)).
|
||||||
|
|
||||||
|
## 원본 자료
|
||||||
|
|
||||||
|
* `resource/ZLAC8015D V4 Series Manual Version 1.03-20251111.pdf` — 하드웨어 매뉴얼
|
||||||
|
* `resource/ZLAC8015D V4 Series RS485 Communication Version 1.06-20251111.pdf` — RS485 통신/레지스터 상세
|
||||||
|
* `resource/ZLAC8015D V4 Series RS485 Communication Quick Start Guide Version 1.00-20251111.pdf` — RS485 빠른 시작 가이드
|
||||||
|
* `resource/ZLAC8015D V4 Series CANopen Communication *.pdf` — CANopen 통신 (본 프로젝트는 RS485만 사용, 참고용)
|
||||||
|
* `resource/ZLAC8015D V4 Series.pdf` — 외형 치수도
|
||||||
|
* `resource/husky.md` — Clearpath Husky User Manual 링크 (스키드 스티어 IMU 융합 제어 비교 참고자료, [06](06-imu-integration-plan.md) 참고)
|
||||||
@@ -0,0 +1,67 @@
|
|||||||
|
# [기능 명세서] RadioMaster Pocket & XR1 V1.0 기반 조종 제어 전환
|
||||||
|
|
||||||
|
## 1. 프로젝트 개요
|
||||||
|
* **목적:** 기존 Xbox 게임패드 기반의 원격 제어 모듈을 **RadioMaster Pocket RC 조종기** 및 **XR1 V1.0 수신기(USB-to-TTL 시리얼 통신)** 기반으로 교체/마이그레이션.
|
||||||
|
* **통신 환경:**
|
||||||
|
* 하드웨어: USB to TTL 컨버터 (`/dev/ttyUSB*` 또는 COM 포트)
|
||||||
|
* 수신기: XR1 V1.0
|
||||||
|
* 조종기: RadioMaster Pocket
|
||||||
|
* **실측 확인된 시리얼 프로토콜: CRSF(Crossfire), 420000bps, 8N1.**
|
||||||
|
프레임 구조 `[Sync 0xC8][Length][FrameType][Payload][CRC8]`, FrameType
|
||||||
|
`0x16`(RC_CHANNELS_PACKED)의 22바이트 payload에 16채널이 11비트씩
|
||||||
|
리틀엔디안으로 패킹되어 raw `172~1811` 값이 `1000~2000us`로 선형
|
||||||
|
변환된다. (CRC8은 이 송신기에서 표준 DVB-S2 값과 안 맞아 검증 없이
|
||||||
|
Sync+Length 프레이밍만 신뢰함.)
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 하드웨어 및 채널 매핑 사양
|
||||||
|
|
||||||
|
### 2.1 채널별 입력 범위 및 기능 정의
|
||||||
|
|
||||||
|
| 채널 | 입력 장치 | Raw Value 범위 | 할당 기능 | 동작 설명 |
|
||||||
|
| :--- | :--- | :--- | :--- | :--- |
|
||||||
|
| **CH1** | 롤/조향 스틱 | • `1005`: 좌측 끝<br>• `1481 ~ 1503`: 중앙 (데드존)<br>• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1481~1503`) 적용<br>• 좌회전(-) / 우회전(+) 정규화 |
|
||||||
|
| **CH2** | 피치/스로틀 스틱 | • `1005`: 최대 후진<br>• `1500`: 중앙 (정지)<br>• `2000`: 최대 전진 | **전후진 (Throttle / Linear X)** | • 값이 높을수록 전진, 낮을수록 후진 (CH1과 반대 방향)<br>• CH6에서 결정된 최대 속도 기준으로 선형 매핑 |
|
||||||
|
| **CH5** | 2단 스위치 | • `1012`: 미작동 (OFF)<br>• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)<br>• `1012` 수신 시 일반 주행 허용 |
|
||||||
|
| **CH6** | 2단 스위치 | • `1832`: 저속 위치<br>• `1012~1090`: 고속 위치 | **속도 모드 선택 (Speed Mode)** | • `1832`(≥1500 부근): 저속 모드 (최대 **0.3 m/s**)<br>• `1012~1090`(<1500): 고속 모드 (최대 **1.5 m/s**) |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 제어 변환 알고리즘 및 로직
|
||||||
|
|
||||||
|
### 3.1 브레이크 최우선 로직 (CH5)
|
||||||
|
* $\text{CH5} \ge 1500$ (누름 / `1988` 부근):
|
||||||
|
$$\text{Linear Velocity} = 0.0, \quad \text{Angular Velocity} = 0.0$$
|
||||||
|
* $\text{CH5} < 1500$ (안누름 / `1012` 부근): 정상 주행 로직 실행
|
||||||
|
|
||||||
|
### 3.2 속도 제한 설정 (CH6)
|
||||||
|
* $\text{CH6} \ge 1500$ (`1832` 부근): $V_{\max} = 0.3 \text{ m/s}$ (저속 모드)
|
||||||
|
* $\text{CH6} < 1500$ (`1012~1090` 부근): $V_{\max} = 1.5 \text{ m/s}$ (고속 모드)
|
||||||
|
|
||||||
|
### 3.3 전후진 속도 계산 (CH2)
|
||||||
|
* **데드존(Deadband):** $1480 \le \text{CH2} \le 1520 \rightarrow V_x = 0.0$
|
||||||
|
* **후진 구간 ($1005 \le \text{CH2} < 1480$):**
|
||||||
|
$$V_x = -\left( \frac{1480 - \text{CH2}}{1480 - 1005} \right) \times V_{\max}$$
|
||||||
|
* **전진 구간 ($1520 < \text{CH2} \le 2000$):**
|
||||||
|
$$V_x = \left( \frac{\text{CH2} - 1520}{2000 - 1520} \right) \times V_{\max}$$
|
||||||
|
|
||||||
|
### 3.4 좌우 조향 각속도 계산 (CH1)
|
||||||
|
* **데드존(Deadband):** $1481 \le \text{CH1} \le 1503 \rightarrow \omega_z = 0.0$
|
||||||
|
* **좌회전 구간 ($1005 \le \text{CH1} < 1481$):**
|
||||||
|
$$\omega_z = -\left( \frac{1481 - \text{CH1}}{1481 - 1005} \right) \times \Omega_{\max}$$
|
||||||
|
* **우회전 구간 ($1503 < \text{CH1} \le 2000$):**
|
||||||
|
$$\omega_z = +\left( \frac{\text{CH1} - 1503}{2000 - 1503} \right) \times \Omega_{\max}$$
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 구현 요구사항 (To Claude Code)
|
||||||
|
|
||||||
|
1. **기존 Xbox 입력 모듈 분리 및 교체:**
|
||||||
|
- 기존의 조이스틱 라이브러리(pygame, evdev, joy 노드 등) 종속성 제거
|
||||||
|
- 시리얼 통신(PySerial 등) 기반의 RC 수신기 데이터 리더 클래스 구현
|
||||||
|
2. **시리얼 패킷 디코딩 & Fail-safe 처리:**
|
||||||
|
- USB-to-TTL 시리얼 포트 오픈 및 예외 처리(재연결 로직)
|
||||||
|
- 패킷 타임아웃(예: 200ms 이상 신호 미수신 시) 발생 시 자동 감속 및 긴급 정지
|
||||||
|
3. **설정값 파라미터화 (Config):**
|
||||||
|
- Serial Port, Baudrate, 데드존 범위, 채널별 Max/Min/Center 값을 외부 설정(YAML, JSON, ROS 파라미터 등)으로 관리 가능하도록 모듈화
|
||||||
@@ -0,0 +1,79 @@
|
|||||||
|
import serial
|
||||||
|
import time
|
||||||
|
|
||||||
|
# 확인된 포트 경로 설정
|
||||||
|
SERIAL_PORT = "/dev/cu.usbserial-0001"
|
||||||
|
BAUD_RATE = 420000
|
||||||
|
|
||||||
|
CRSF_SYNC = 0xC8
|
||||||
|
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16
|
||||||
|
|
||||||
|
def parse_channels(payload):
|
||||||
|
if len(payload) < 22:
|
||||||
|
return []
|
||||||
|
|
||||||
|
# 22바이트의 채널 데이터를 정수로 변환 후 11비트씩 언패킹
|
||||||
|
data = int.from_bytes(payload[:22], byteorder='little')
|
||||||
|
channels = []
|
||||||
|
for _ in range(16):
|
||||||
|
raw_val = data & 0x07FF # 11-bit 마스크
|
||||||
|
# CRSF 원본 값(172~1811)을 RC 마이크로초(1000~2000µs) 단위로 변환
|
||||||
|
us_val = int((raw_val - 172) * (2000 - 1000) / (1811 - 172) + 1000)
|
||||||
|
channels.append(us_val)
|
||||||
|
data >>= 11
|
||||||
|
return channels
|
||||||
|
|
||||||
|
def main():
|
||||||
|
try:
|
||||||
|
ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=0.1)
|
||||||
|
print(f"Connected to {SERIAL_PORT} at {BAUD_RATE} baud.")
|
||||||
|
print("스틱과 스위치를 움직여 채널 값을 확인하세요. (종료: Ctrl+C)\n")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"포트 연결 오류: {e}")
|
||||||
|
return
|
||||||
|
|
||||||
|
buf = bytearray()
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
if ser.in_waiting:
|
||||||
|
buf.extend(ser.read(ser.in_waiting))
|
||||||
|
|
||||||
|
while len(buf) > 3:
|
||||||
|
if buf[0] != CRSF_SYNC:
|
||||||
|
buf.pop(0)
|
||||||
|
continue
|
||||||
|
|
||||||
|
length = buf[1]
|
||||||
|
total_packet_len = length + 2
|
||||||
|
|
||||||
|
if len(buf) < total_packet_len:
|
||||||
|
break # 전체 패킷 대기
|
||||||
|
|
||||||
|
packet = buf[:total_packet_len]
|
||||||
|
buf = buf[total_packet_len:]
|
||||||
|
|
||||||
|
frame_type = packet[2]
|
||||||
|
if frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
|
||||||
|
payload = packet[3:-1] # CRC 제외 payload
|
||||||
|
channels = parse_channels(payload)
|
||||||
|
|
||||||
|
# read_crsf.py 출력 부분 수정
|
||||||
|
print(
|
||||||
|
f"\rCH1: {channels[0]:4d} | CH2: {channels[1]:4d} | "
|
||||||
|
f"CH3: {channels[2]:4d} | CH4: {channels[3]:4d} | "
|
||||||
|
f"CH5(SA/SC): {channels[4]:4d} | CH6(SD): {channels[5]:4d} | "
|
||||||
|
f"CH7: {channels[6]:4d} | CH8: {channels[7]:4d}",
|
||||||
|
end="", flush=True
|
||||||
|
)
|
||||||
|
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n모니터링 종료")
|
||||||
|
break
|
||||||
|
except Exception as e:
|
||||||
|
print(f"\n데이터 수신 오류: {e}")
|
||||||
|
break
|
||||||
|
|
||||||
|
ser.close()
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,55 @@
|
|||||||
|
RELEASE NOTES
|
||||||
|
=============
|
||||||
|
WITMOTION Turotial File
|
||||||
|
|
||||||
|
Package version: 2023.06.26
|
||||||
|
Release date: 2023-06-26
|
||||||
|
|
||||||
|
This package contains all necessary files for set-up of the sensor.
|
||||||
|
|
||||||
|
Unzip the .zip file and you will have a directory containing the files.
|
||||||
|
|
||||||
|
-----------
|
||||||
|
Website:
|
||||||
|
https://www.wit-motion.com/
|
||||||
|
|
||||||
|
Document download link:
|
||||||
|
|
||||||
|
Google Drive:
|
||||||
|
https://drive.google.com/open?id=1s-7NIhHXFyLhF8TmCFdrVPz80SwS3C1i
|
||||||
|
|
||||||
|
-----------
|
||||||
|
Youtube Channel:
|
||||||
|
https://www.youtube.com/c/WITMOTION
|
||||||
|
|
||||||
|
HWT905-RS232 Playlist:
|
||||||
|
https://youtube.com/playlist?list=PL43tdDrVL_VC49nv2WW4NphnUTxSkQ_cZ
|
||||||
|
|
||||||
|
-----------
|
||||||
|
After-sale Service& Technical Support:
|
||||||
|
If you have queries about the sensors' set-up, software installation, APP, drivers
|
||||||
|
or any other questions, suggestions, please feel free to contact us.
|
||||||
|
|
||||||
|
Our engineering team is committed to providing the required support necessary
|
||||||
|
to ensure that you are satisfied with WITMOTION sensor and the service.
|
||||||
|
|
||||||
|
1. Please find the below link of detailed contact method (support@wit-motion.com)
|
||||||
|
https://www.wit-motion.com/contacts/
|
||||||
|
|
||||||
|
2. Double-confirmed which platform you bought and
|
||||||
|
contact the corresponding team by the provided way
|
||||||
|
|
||||||
|
3. Sending a message including the problem and necessary description
|
||||||
|
would be highly helpful to give a workable solution with details
|
||||||
|
|
||||||
|
4. Leave a note indicating the platform you placed the order,
|
||||||
|
the order ID, so it is intuitive to check the model you purchase
|
||||||
|
and accessory you may buy
|
||||||
|
|
||||||
|
5. For a complicated issue like calibration, PIN connection, configurations,
|
||||||
|
it is recommended to provide necessary files including screenshots,
|
||||||
|
taking several pictures, or shooting videos. A short video including the PIN connection,
|
||||||
|
software setup, operation details would be preferred.
|
||||||
|
It would be further help for remote diagnostics.
|
||||||
|
|
||||||
|
|
||||||
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1 @@
|
|||||||
|
https://docs.clearpathrobotics.com/docs_robots/legacy/ros1_robots/outdoor_robots/husky/user_manual_husky
|
||||||
+14
-9
@@ -1,7 +1,9 @@
|
|||||||
#!/usr/bin/env bash
|
#!/usr/bin/env bash
|
||||||
|
|
||||||
# 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance
|
# 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)"
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
|
|
||||||
cd "$SCRIPT_DIR" || exit 1
|
cd "$SCRIPT_DIR" || exit 1
|
||||||
@@ -10,10 +12,11 @@ echo "=================================================================="
|
|||||||
echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템"
|
echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템"
|
||||||
echo "=================================================================="
|
echo "=================================================================="
|
||||||
|
|
||||||
# 1. USB Latency 1ms 단축
|
# 1. USB Latency 1ms 단축 (모터 포트 + RC 수신기 포트 둘 다)
|
||||||
if [ -f "./set_low_latency.sh" ]; then
|
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 "$PORT"
|
||||||
|
./set_low_latency.sh "$RC_PORT"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# 2. C++ 바이너리 존재 여부 확인 및 컴파일
|
# 2. C++ 바이너리 존재 여부 확인 및 컴파일
|
||||||
@@ -23,14 +26,16 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then
|
|||||||
fi
|
fi
|
||||||
|
|
||||||
echo "=================================================================="
|
echo "=================================================================="
|
||||||
echo " [시작] C++ 100Hz 초저지연 4WD 조이스틱 + 라이다 장애물 회피 시작 ($PORT)"
|
echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작"
|
||||||
echo " - 라이다 기능 토글: Xbox 컨트롤러 X 버튼"
|
echo " - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT"
|
||||||
echo " - 정지/종료: B 버튼 또는 Ctrl+C"
|
echo " - 라이다 기능 비활성화: --no_lidar 옵션"
|
||||||
|
echo " - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C"
|
||||||
echo "=================================================================="
|
echo "=================================================================="
|
||||||
|
|
||||||
# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능)
|
# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port를 다시 넘기면
|
||||||
|
# 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨)
|
||||||
if [ $# -gt 1 ]; then
|
if [ $# -gt 1 ]; then
|
||||||
./xbox_motor_control_cpp --port "$PORT" "${@:2}"
|
./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" "${@:2}"
|
||||||
else
|
else
|
||||||
./xbox_motor_control_cpp --port "$PORT"
|
./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT"
|
||||||
fi
|
fi
|
||||||
|
|||||||
+569
-141
@@ -1,12 +1,14 @@
|
|||||||
#include <SDL2/SDL.h>
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
#include <array>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
|
#include <cerrno>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <csignal>
|
#include <csignal>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
#include <map>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
@@ -601,34 +603,330 @@ public:
|
|||||||
|
|
||||||
LidarObstacleDetector *LidarObstacleDetector::instance_ = nullptr;
|
LidarObstacleDetector *LidarObstacleDetector::instance_ = nullptr;
|
||||||
|
|
||||||
float applyDeadzone(float val, float deadzone = 0.1f) {
|
// -----------------------------------------------------------------------------
|
||||||
if (std::abs(val) < deadzone)
|
// RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈
|
||||||
return 0.0f;
|
// 실기 캡처로 확인한 실제 프로토콜: CRSF(Crossfire), 420000bps, 8N1.
|
||||||
float sign = (val > 0.0f) ? 1.0f : -1.0f;
|
// 프레임: [0]=Sync(0xC8) [1]=Length [2]=FrameType [3..]=Payload [-1]=CRC8
|
||||||
float norm = (std::abs(val) - deadzone) / (1.0f - deadzone);
|
// FrameType 0x16(RC_CHANNELS_PACKED)의 22바이트 payload에 16채널이 11비트씩
|
||||||
return sign * norm;
|
// 리틀엔디안으로 패킹되어 있다 (raw 172~1811 -> 1000~2000us로 선형 변환).
|
||||||
|
// 참고: mac/mac_code (맥에서 정상 동작 확인된 레퍼런스 파이썬 스크립트)와
|
||||||
|
// 동일한 파싱 방식을 따른다 — 실측 결과 CRC8이 표준 DVB-S2(poly 0xD5) 값과
|
||||||
|
// 맞지 않아(송신기별 구현 차이로 추정) 레퍼런스와 동일하게 CRC는 검증하지
|
||||||
|
// 않고 Sync+Length 기반 프레이밍만 신뢰한다.
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
#ifndef BOTHER
|
||||||
|
#define BOTHER 0010000 // 커널 termbits.h: CBAUDEX와 동일값, 임의 보드레이트 지정용
|
||||||
|
#endif
|
||||||
|
struct termios2 { // <asm/termbits.h>의 커널 ABI와 동일 레이아웃 (glibc의
|
||||||
|
// struct termios와 이름 충돌을 피하기 위해 직접 선언).
|
||||||
|
// 420000bps처럼 표준 B-상수가 없는 보드레이트를
|
||||||
|
// TCSETS2/BOTHER ioctl로 설정하기 위해 필요하다.
|
||||||
|
tcflag_t c_iflag, c_oflag, c_cflag, c_lflag;
|
||||||
|
cc_t c_line;
|
||||||
|
cc_t c_cc[19];
|
||||||
|
speed_t c_ispeed, c_ospeed;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct RcConfig {
|
||||||
|
std::string port = "/dev/ttyUSB0"; // 현재 고정 배선: RC 수신기(XR1) = ttyUSB0
|
||||||
|
int baud = 420000; // CRSF 표준 보드레이트
|
||||||
|
|
||||||
|
int ch_steer = 1; // CH1: 좌우 조향
|
||||||
|
int ch_throttle = 2; // CH2: 전후진
|
||||||
|
int ch_brake = 5; // CH5: 비상 브레이크
|
||||||
|
int ch_speed = 6; // CH6: 속도 모드
|
||||||
|
|
||||||
|
// 실기 캡처로 확인한 실측값 (좌 1005 ~ 중 1481~1503 ~ 우 2000)
|
||||||
|
int steer_min = 1005, steer_center_lo = 1481, steer_center_hi = 1503,
|
||||||
|
steer_max = 2000;
|
||||||
|
// 실측값: 후진끝 1005 ~ 중 1500 ~ 전진끝 2000 (CH2는 값이 높을수록 전진 —
|
||||||
|
// CH1과 반대 방향이라 아래 3.3 매핑 로직도 그에 맞춰 부호가 반대다)
|
||||||
|
int throttle_min = 1005, throttle_center_lo = 1480, throttle_center_hi = 1520,
|
||||||
|
throttle_max = 2000;
|
||||||
|
|
||||||
|
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 고속 모드)
|
||||||
|
|
||||||
|
int failsafe_timeout_ms = 200; // 이 시간 이상 신호 미수신 시 비상정지
|
||||||
|
};
|
||||||
|
|
||||||
|
class RcReceiver {
|
||||||
|
public:
|
||||||
|
explicit RcReceiver(const RcConfig &cfg) : cfg_(cfg) {}
|
||||||
|
~RcReceiver() { stop(); }
|
||||||
|
|
||||||
|
void start() {
|
||||||
|
running_ = true;
|
||||||
|
thread_ = std::thread(&RcReceiver::run, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void stop() {
|
||||||
|
running_ = false;
|
||||||
|
if (thread_.joinable())
|
||||||
|
thread_.join();
|
||||||
|
closePort();
|
||||||
|
}
|
||||||
|
|
||||||
|
// 채널 raw 값 조회. 아직 한 번도 수신하지 못했으면 false.
|
||||||
|
bool getChannel(int ch, int &out_val) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
auto it = channels_.find(ch);
|
||||||
|
if (it == channels_.end())
|
||||||
|
return false;
|
||||||
|
out_val = it->second;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 포트가 열려 있고, failsafe_timeout_ms 이내에 유효한 패킷을 수신했는가.
|
||||||
|
bool isConnected() {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (!port_open_ || channels_.empty())
|
||||||
|
return false;
|
||||||
|
auto now = std::chrono::steady_clock::now();
|
||||||
|
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||||
|
now - last_rx_time_)
|
||||||
|
.count() <= cfg_.failsafe_timeout_ms;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
RcConfig cfg_;
|
||||||
|
std::atomic<bool> running_{false};
|
||||||
|
std::thread thread_;
|
||||||
|
int fd_ = -1;
|
||||||
|
|
||||||
|
std::mutex mutex_;
|
||||||
|
std::map<int, int> channels_;
|
||||||
|
std::chrono::steady_clock::time_point last_rx_time_{};
|
||||||
|
bool port_open_ = false;
|
||||||
|
|
||||||
|
std::vector<uint8_t> frame_buf_;
|
||||||
|
|
||||||
|
// 채널별 미디언(median-of-3) 필터 히스토리. 이 송신기는 CRC/체크섬이
|
||||||
|
// 표준 방식으로 검증되지 않아(전수조사로 확인) 단발성 손상 프레임이
|
||||||
|
// 섞여 들어올 수 있는데, 최근 3개 raw 값 중 가운데 값을 채택하면 3개
|
||||||
|
// 중 하나가 손상돼도 자동으로 걸러진다. 스위치 채널도 전환 시 최대
|
||||||
|
// 1프레임(~4ms)만 지연될 뿐이라 안전하게 전 채널에 적용 가능하다.
|
||||||
|
std::map<int, std::array<int, 2>> raw_history_;
|
||||||
|
std::map<int, int> 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; // 스펙상 프레임은 훨씬 작음
|
||||||
|
|
||||||
|
bool openPort() {
|
||||||
|
fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
||||||
|
if (fd_ < 0)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
fcntl(fd_, F_SETFL, O_NONBLOCK);
|
||||||
|
|
||||||
|
// CRSF 표준 420000bps는 termios의 표준 B-상수 목록에 없어 TCSETS2 +
|
||||||
|
// BOTHER + c_ispeed/c_ospeed(임의 정수 보드레이트) ioctl로 설정한다.
|
||||||
|
struct termios2 tio;
|
||||||
|
if (ioctl(fd_, TCGETS2, &tio) < 0) {
|
||||||
|
close(fd_);
|
||||||
|
fd_ = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
tio.c_cflag &= ~CBAUD;
|
||||||
|
tio.c_cflag |= BOTHER;
|
||||||
|
tio.c_ispeed = static_cast<speed_t>(cfg_.baud);
|
||||||
|
tio.c_ospeed = static_cast<speed_t>(cfg_.baud);
|
||||||
|
|
||||||
|
tio.c_cflag &= ~PARENB;
|
||||||
|
tio.c_cflag &= ~CSTOPB;
|
||||||
|
tio.c_cflag &= ~CSIZE;
|
||||||
|
tio.c_cflag |= CS8;
|
||||||
|
tio.c_cflag &= ~CRTSCTS;
|
||||||
|
tio.c_cflag |= CREAD | CLOCAL;
|
||||||
|
|
||||||
|
// 바이너리 프로토콜이므로 raw 모드(비정규/논캐노니컬)로 읽는다.
|
||||||
|
tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | IEXTEN);
|
||||||
|
tio.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
|
||||||
|
ISTRIP | INLCR | IGNCR | ICRNL);
|
||||||
|
tio.c_oflag &= ~OPOST;
|
||||||
|
tio.c_cc[VMIN] = 0;
|
||||||
|
tio.c_cc[VTIME] = 0;
|
||||||
|
|
||||||
|
if (ioctl(fd_, TCSETS2, &tio) < 0) {
|
||||||
|
close(fd_);
|
||||||
|
fd_ = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
tcflush(fd_, TCIOFLUSH);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void closePort() {
|
||||||
|
if (fd_ >= 0) {
|
||||||
|
close(fd_);
|
||||||
|
fd_ = -1;
|
||||||
|
}
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
port_open_ = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 축적된 바이트 버퍼에서 CRSF 프레임을 최대한 뽑아낸다. Sync 바이트를
|
||||||
|
// 찾고, 선언된 Length만큼 버퍼에 데이터가 쌓일 때까지 기다린 뒤
|
||||||
|
// RC_CHANNELS_PACKED(0x16) 프레임이면 16채널(11비트씩)을 언패킹한다.
|
||||||
|
// (CRC8은 이 송신기에서 표준 DVB-S2 값과 맞지 않아 검증하지 않음 — 실기
|
||||||
|
// 캡처로 확인된 mac/mac_code 레퍼런스와 동일한 방식.)
|
||||||
|
void parseCrsfBuffer() {
|
||||||
|
std::map<int, int> parsed;
|
||||||
|
size_t i = 0;
|
||||||
|
while (i < frame_buf_.size()) {
|
||||||
|
if (frame_buf_[i] != CRSF_SYNC) {
|
||||||
|
++i;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (i + 1 >= frame_buf_.size())
|
||||||
|
break; // length 바이트 대기
|
||||||
|
|
||||||
|
uint8_t length = frame_buf_[i + 1];
|
||||||
|
size_t total_len = static_cast<size_t>(length) + 2;
|
||||||
|
if (length < 2 || total_len > CRSF_MAX_FRAME_LEN) {
|
||||||
|
// Sync처럼 보이는 노이즈 바이트: 1바이트만 버리고 재동기화
|
||||||
|
++i;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (i + total_len > frame_buf_.size())
|
||||||
|
break; // 프레임 전체 수신 대기
|
||||||
|
|
||||||
|
uint8_t frame_type = frame_buf_[i + 2];
|
||||||
|
if (frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED && length >= 23) {
|
||||||
|
// payload = frame_buf_[i+3 .. i+total_len-2] (CRC 바이트 제외 22바이트
|
||||||
|
// = 176비트 = 16채널×11비트). uint64_t 하나에 통째로 담으면 64비트를
|
||||||
|
// 넘는 시프트가 발생해 정의되지 않은 동작(UB)이 되므로, 32비트 롤링
|
||||||
|
// 비트 누산기로 바이트를 하나씩 채워가며 11비트씩 뽑아낸다.
|
||||||
|
uint32_t bit_accum = 0;
|
||||||
|
int bit_count = 0;
|
||||||
|
int byte_idx = 0;
|
||||||
|
for (int ch = 0; ch < 16; ++ch) {
|
||||||
|
while (bit_count < 11) {
|
||||||
|
bit_accum |= static_cast<uint32_t>(frame_buf_[i + 3 + byte_idx])
|
||||||
|
<< bit_count;
|
||||||
|
bit_count += 8;
|
||||||
|
++byte_idx;
|
||||||
|
}
|
||||||
|
int raw_val = static_cast<int>(bit_accum & 0x7FFU);
|
||||||
|
bit_accum >>= 11;
|
||||||
|
bit_count -= 11;
|
||||||
|
int us_val = static_cast<int>(
|
||||||
|
std::lround((raw_val - 172) * 1000.0 / 1639.0 + 1000.0));
|
||||||
|
|
||||||
|
// 미디언-3 필터: 최근 2개 raw 값 + 이번 값 중 가운데 값을 채택.
|
||||||
|
// 히스토리는 항상 raw 값으로 갱신한다(필터링된 결과가 아니라).
|
||||||
|
int ch_num = ch + 1;
|
||||||
|
std::array<int, 2> &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;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 처리하지 못한 잔여 바이트(다음 프레임 일부)만 남기고 버퍼 정리
|
||||||
|
frame_buf_.erase(frame_buf_.begin(), frame_buf_.begin() + i);
|
||||||
|
if (frame_buf_.size() > CRSF_MAX_FRAME_LEN * 4) // 비정상 누적 방지
|
||||||
|
frame_buf_.clear();
|
||||||
|
|
||||||
|
if (!parsed.empty()) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
for (auto &kv : parsed)
|
||||||
|
channels_[kv.first] = kv.second;
|
||||||
|
last_rx_time_ = std::chrono::steady_clock::now();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void run() {
|
||||||
|
// now() - 1001ms로 초기화: time_point::min()을 쓰면 첫 비교에서
|
||||||
|
// "now - sentinel"이 부호있는 정수 오버플로우(UB)를 일으켜 -O3에서
|
||||||
|
// 재연결 시도 자체가 영구히 스킵될 수 있다.
|
||||||
|
auto last_open_attempt =
|
||||||
|
std::chrono::steady_clock::now() - std::chrono::milliseconds(1001);
|
||||||
|
char buf[256];
|
||||||
|
|
||||||
|
while (running_) {
|
||||||
|
if (fd_ < 0) {
|
||||||
|
auto now = std::chrono::steady_clock::now();
|
||||||
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||||
|
now - last_open_attempt)
|
||||||
|
.count() > 1000) {
|
||||||
|
last_open_attempt = now;
|
||||||
|
if (openPort()) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
port_open_ = true;
|
||||||
|
std::cout << "\n[정보] RC 수신기 포트(" << cfg_.port
|
||||||
|
<< ") 연결 성공.\n";
|
||||||
|
} else {
|
||||||
|
std::cerr << "\n[경고] RC 수신기 포트(" << cfg_.port
|
||||||
|
<< ") 열기 실패. 1초 후 재시도.\n";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t n = read(fd_, buf, sizeof(buf));
|
||||||
|
if (n > 0) {
|
||||||
|
frame_buf_.insert(frame_buf_.end(), buf, buf + n);
|
||||||
|
parseCrsfBuffer();
|
||||||
|
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
|
||||||
|
std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n";
|
||||||
|
closePort();
|
||||||
|
} else {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
int main(int argc, char **argv) {
|
int main(int argc, char **argv) {
|
||||||
std::signal(SIGINT, signalHandler);
|
std::signal(SIGINT, signalHandler);
|
||||||
std::signal(SIGTERM, signalHandler);
|
std::signal(SIGTERM, signalHandler);
|
||||||
|
|
||||||
std::string port1 = "/dev/ttyUSB0";
|
std::string port1 = "/dev/ttyUSB1"; // 현재 고정 배선: 모터 드라이버 = ttyUSB1
|
||||||
std::string port2 = "";
|
std::string port2 = "";
|
||||||
uint8_t id1 = 1;
|
uint8_t id1 = 1;
|
||||||
uint8_t id2 = 2;
|
uint8_t id2 = 2;
|
||||||
bool bcast_mode = false;
|
bool bcast_mode = false;
|
||||||
float max_v = 0.40f; // m/s
|
RcConfig rc_cfg;
|
||||||
float max_spin_v = 0.30f; // m/s
|
float max_spin_v = 0.30f; // m/s
|
||||||
float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm
|
float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm
|
||||||
float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm
|
float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm
|
||||||
float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름
|
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 decel_rate = 180.0f; // RPM/s (최대 감속도 한계)
|
||||||
float jerk_rate = 600.0f; // RPM/s^2 (직진/커브 선회 저크 한계)
|
float jerk_rate = 600.0f; // RPM/s^2 (직진/커브 선회 감속 저크 한계)
|
||||||
float spin_jerk_rate = 220.0f; // RPM/s^2 (제자리 회전 저크 한계: 고마찰 노면에서
|
float spin_jerk_rate = 220.0f; // RPM/s^2 (제자리 회전 감속 저크 한계)
|
||||||
// 정지마찰이 급격히 파괴되지 않도록 accel_rate에
|
|
||||||
// 도달하는 시간을 더 길게 늘림)
|
|
||||||
float spin_arc_bias = 0.12f; // 0~1 (제자리 회전 시 섞을 최소 회전반경용
|
float spin_arc_bias = 0.12f; // 0~1 (제자리 회전 시 섞을 최소 회전반경용
|
||||||
// 미세 전진 성분 비율. ICR을 로봇 중심에서
|
// 미세 전진 성분 비율. ICR을 로봇 중심에서
|
||||||
// 살짝 벗어나게 해 스크럽 마찰 저항을 줄임)
|
// 살짝 벗어나게 해 스크럽 마찰 저항을 줄임)
|
||||||
@@ -690,8 +988,6 @@ int main(int argc, char **argv) {
|
|||||||
wheel_radius = std::stof(argv[++i]);
|
wheel_radius = std::stof(argv[++i]);
|
||||||
else if (arg == "--bumper" && i + 1 < argc)
|
else if (arg == "--bumper" && i + 1 < argc)
|
||||||
bumper_offset = std::stof(argv[++i]);
|
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)
|
else if (arg == "--decel" && i + 1 < argc)
|
||||||
decel_rate = std::stof(argv[++i]);
|
decel_rate = std::stof(argv[++i]);
|
||||||
else if (arg == "--jerk" && i + 1 < argc)
|
else if (arg == "--jerk" && i + 1 < argc)
|
||||||
@@ -738,6 +1034,48 @@ int main(int argc, char **argv) {
|
|||||||
max_z = std::stof(argv[++i]);
|
max_z = std::stof(argv[++i]);
|
||||||
else if (arg == "--no_lidar")
|
else if (arg == "--no_lidar")
|
||||||
use_lidar = false;
|
use_lidar = false;
|
||||||
|
else if (arg == "--rc_port" && i + 1 < argc)
|
||||||
|
rc_cfg.port = argv[++i];
|
||||||
|
else if (arg == "--rc_baud" && i + 1 < argc)
|
||||||
|
rc_cfg.baud = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_ch_steer" && i + 1 < argc)
|
||||||
|
rc_cfg.ch_steer = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_ch_throttle" && i + 1 < argc)
|
||||||
|
rc_cfg.ch_throttle = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_ch_brake" && i + 1 < argc)
|
||||||
|
rc_cfg.ch_brake = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_ch_speed" && i + 1 < argc)
|
||||||
|
rc_cfg.ch_speed = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_steer_min" && i + 1 < argc)
|
||||||
|
rc_cfg.steer_min = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_steer_center_lo" && i + 1 < argc)
|
||||||
|
rc_cfg.steer_center_lo = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_steer_center_hi" && i + 1 < argc)
|
||||||
|
rc_cfg.steer_center_hi = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_steer_max" && i + 1 < argc)
|
||||||
|
rc_cfg.steer_max = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_throttle_min" && i + 1 < argc)
|
||||||
|
rc_cfg.throttle_min = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_throttle_center_lo" && i + 1 < argc)
|
||||||
|
rc_cfg.throttle_center_lo = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_throttle_center_hi" && i + 1 < argc)
|
||||||
|
rc_cfg.throttle_center_hi = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_throttle_max" && i + 1 < argc)
|
||||||
|
rc_cfg.throttle_max = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--rc_brake_threshold" && i + 1 < argc)
|
||||||
|
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)
|
||||||
|
rc_cfg.v_max_high = std::stof(argv[++i]);
|
||||||
|
else if (arg == "--rc_failsafe_ms" && i + 1 < argc)
|
||||||
|
rc_cfg.failsafe_timeout_ms = std::stoi(argv[++i]);
|
||||||
|
else if (arg == "--max_spin_v" && i + 1 < argc)
|
||||||
|
max_spin_v = std::stof(argv[++i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
constexpr float PI_VAL = 3.14159265358979323846f;
|
constexpr float PI_VAL = 3.14159265358979323846f;
|
||||||
@@ -761,10 +1099,16 @@ int main(int argc, char **argv) {
|
|||||||
std::cout << " - 필터링 범위: 높이 Z(" << min_z << "m ~ " << max_z
|
std::cout << " - 필터링 범위: 높이 Z(" << min_z << "m ~ " << max_z
|
||||||
<< "m) | 회피 정밀 전폭 " << (robot_width + 0.10f) * 1000.0f
|
<< "m) | 회피 정밀 전폭 " << (robot_width + 0.10f) * 1000.0f
|
||||||
<< "mm\n";
|
<< "mm\n";
|
||||||
std::cout << " - 최고 속도: " << max_v << "m/s | 가속: " << accel_rate
|
std::cout << " - 최고 속도: 저속 " << rc_cfg.v_max_low << "m/s / 고속 "
|
||||||
<< "RPM/s | Jerk: " << jerk_rate << "RPM/s² (제자리회전: "
|
<< rc_cfg.v_max_high << "m/s (CH" << rc_cfg.ch_speed
|
||||||
<< spin_jerk_rate << "RPM/s²) | 회전 곡선혼합: "
|
<< " 전환) | 가속: 즉시 추종(1:1) | 감속 Jerk: " << jerk_rate
|
||||||
<< spin_arc_bias * 100.0f << "%\n";
|
<< "RPM/s² (제자리회전: " << spin_jerk_rate
|
||||||
|
<< "RPM/s²) | 회전 곡선혼합: " << spin_arc_bias * 100.0f << "%\n";
|
||||||
|
std::cout << " - RC 수신기: " << rc_cfg.port << " @" << rc_cfg.baud
|
||||||
|
<< "bps | 조향 CH" << rc_cfg.ch_steer << " | 스로틀 CH"
|
||||||
|
<< rc_cfg.ch_throttle << " | 브레이크 CH" << rc_cfg.ch_brake
|
||||||
|
<< " | 속도모드 CH" << rc_cfg.ch_speed
|
||||||
|
<< " | Fail-safe: " << rc_cfg.failsafe_timeout_ms << "ms\n";
|
||||||
std::cout << " - 휠 이상판정: 무부하(들뜸) 전류비 " << airborne_current_ratio
|
std::cout << " - 휠 이상판정: 무부하(들뜸) 전류비 " << airborne_current_ratio
|
||||||
<< " | 걸림 속도비 " << stall_vel_ratio << " / 전류 "
|
<< " | 걸림 속도비 " << stall_vel_ratio << " / 전류 "
|
||||||
<< stall_current_a << "A 이상\n";
|
<< stall_current_a << "A 이상\n";
|
||||||
@@ -783,7 +1127,8 @@ int main(int argc, char **argv) {
|
|||||||
if (!log_path.empty()) {
|
if (!log_path.empty()) {
|
||||||
log_stream.open(log_path);
|
log_stream.open(log_path);
|
||||||
if (log_stream.is_open()) {
|
if (log_stream.is_open()) {
|
||||||
log_stream << "t_ms,state,ly,lx,rx,"
|
log_stream << "t_ms,state,rc_steer,rc_throttle,rc_ok,brake,vmax,vx,omega,"
|
||||||
|
"ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,"
|
||||||
"cmd_fl,cmd_fr,cmd_rl,cmd_rr,"
|
"cmd_fl,cmd_fr,cmd_rl,cmd_rr,"
|
||||||
"fb_fl,fb_fr,fb_rl,fb_rr,"
|
"fb_fl,fb_fr,fb_rl,fb_rr,"
|
||||||
"amp_fl,amp_fr,amp_rl,amp_rr,"
|
"amp_fl,amp_fr,amp_rl,amp_rr,"
|
||||||
@@ -809,26 +1154,23 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (SDL_Init(SDL_INIT_JOYSTICK) < 0) {
|
// 모터(--port)와 RC 수신기(--rc_port)가 같은 물리 포트를 가리키면 두
|
||||||
std::cerr << "[오류] SDL 조이스틱 초기화 실패: " << SDL_GetError()
|
// 스레드가 같은 장치의 read()를 두고 경쟁해 RC 라인 파싱이 깨진다.
|
||||||
<< std::endl;
|
// 흔한 실수(예: 모터 미연결 상태에서 RC 수신기가 ttyUSB0에 꽂혀있는데
|
||||||
return 1;
|
// --rc_port를 기본값 ttyUSB1로 둔 채 실행)를 바로 알아차리도록 경고한다.
|
||||||
|
if (port1 == rc_cfg.port) {
|
||||||
|
std::cout << "\n[경고] --port(모터, " << port1 << ")와 --rc_port(RC 수신기, "
|
||||||
|
<< rc_cfg.port
|
||||||
|
<< ")가 동일한 포트로 설정되어 있습니다. 두 기능이 같은 장치를 "
|
||||||
|
"동시에 읽으면 RC 데이터 파싱이 깨질 수 있으니 서로 다른 "
|
||||||
|
"포트로 지정하세요.\n\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
if (SDL_NumJoysticks() < 1) {
|
RcReceiver rc(rc_cfg);
|
||||||
std::cerr << "[경고] 무선 Xbox 컨트롤러(조이스틱)가 연결되지 않았습니다.\n";
|
rc.start();
|
||||||
SDL_Quit();
|
std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 ("
|
||||||
return 1;
|
<< rc_cfg.port << "). 신호 수신 전까지는 안전을 위해 브레이크가 "
|
||||||
}
|
"걸린 상태로 대기합니다.\n";
|
||||||
|
|
||||||
SDL_Joystick *joystick = SDL_JoystickOpen(0);
|
|
||||||
if (!joystick) {
|
|
||||||
std::cerr << "[오류] 조이스틱 열기 실패!\n";
|
|
||||||
SDL_Quit();
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
std::cout << "[정보] C++ Xbox 컨트롤러 연결 성공: "
|
|
||||||
<< SDL_JoystickName(joystick) << std::endl;
|
|
||||||
|
|
||||||
SerialPort sp1, sp2;
|
SerialPort sp1, sp2;
|
||||||
if (!sp1.openPort(port1))
|
if (!sp1.openPort(port1))
|
||||||
@@ -887,8 +1229,9 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 "
|
std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: Ctrl+C, "
|
||||||
"또는 Ctrl+C, 라이다 토글: X 버튼)\n\n";
|
"라이다 회피 " << (use_lidar ? "활성화" : "비활성화")
|
||||||
|
<< " 상태로 시작 - 변경하려면 --no_lidar 옵션을 사용하세요)\n\n";
|
||||||
|
|
||||||
float target_fl = 0.0f, target_fr = 0.0f;
|
float target_fl = 0.0f, target_fr = 0.0f;
|
||||||
float target_rl = 0.0f, target_rr = 0.0f;
|
float target_rl = 0.0f, target_rr = 0.0f;
|
||||||
@@ -902,13 +1245,14 @@ int main(int argc, char **argv) {
|
|||||||
const float loop_hz = 100.0f;
|
const float loop_hz = 100.0f;
|
||||||
const float dt = 1.0f / loop_hz;
|
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,
|
auto jerkLimitedStep = [dt](float current_vel, float target_vel,
|
||||||
float ¤t_accel, float max_accel_mag,
|
float ¤t_accel, float max_decel_mag,
|
||||||
float max_decel_mag, float jerk_limit) {
|
float jerk_limit) {
|
||||||
float diff = target_vel - current_vel;
|
float diff = target_vel - current_vel;
|
||||||
if (std::abs(diff) < 0.01f) {
|
if (std::abs(diff) < 0.01f) {
|
||||||
current_accel = 0.0f;
|
current_accel = 0.0f;
|
||||||
@@ -916,9 +1260,13 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool is_accelerating = std::abs(target_vel) > std::abs(current_vel);
|
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를 없애는데 필요한 가속도(부호 포함)를 물리적 한계로 클램프
|
// 이번 틱에 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;
|
float max_delta_accel = jerk_limit * dt;
|
||||||
current_accel = std::clamp(desired_accel, current_accel - max_delta_accel,
|
current_accel = std::clamp(desired_accel, current_accel - max_delta_accel,
|
||||||
@@ -943,48 +1291,122 @@ int main(int argc, char **argv) {
|
|||||||
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 해제) 직후 한 틱 동안: 차체는 이미 그 속도로 움직이고
|
// 착지(airborne 해제) 직후 한 틱 동안: 차체는 이미 그 속도로 움직이고
|
||||||
// 있는데 cmd는 정지출발용 accel_rate로 천천히 재가속되면, 그 사이 바퀴가
|
// 있는데 cmd가 뒤늦게 target을 쫓아가면 그 사이 바퀴가 지면 이동속도를
|
||||||
// 지면 이동속도를 못 따라가 끌리며 긁히는 문제가 있어 즉시 target으로
|
// 못 따라가 끌리며 긁히는 문제가 있어 즉시 target으로 스냅시켜 지연을
|
||||||
// 스냅시켜 지연을 없앤다.
|
// 없앤다.
|
||||||
bool just_landed_fl = false, just_landed_fr = false;
|
bool just_landed_fl = false, just_landed_fr = false;
|
||||||
bool just_landed_rl = false, just_landed_rr = 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; // 드라이버 알람(과전류/과부하 등) 발생 여부
|
||||||
|
bool prev_brake_on = false; // 브레이크(CH5/Fail-safe) 직전 틱 상태
|
||||||
|
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) {
|
while (g_running) {
|
||||||
SDL_Event event;
|
// ---------------------------------------------------------------------
|
||||||
while (SDL_PollEvent(&event)) {
|
// RC 수신기 채널 읽기 (doc/joystick.md §2, §3 기능 명세서 기준)
|
||||||
if (event.type == SDL_JOYBUTTONDOWN) {
|
// ---------------------------------------------------------------------
|
||||||
if (event.jbutton.button == 0) { // A 버튼: 트립 리셋
|
int raw_steer = rc_cfg.steer_center_lo, raw_throttle = 1500;
|
||||||
trip_initialized = false;
|
int raw_brake = 1011, raw_speed_mode = 1011;
|
||||||
} else if (event.jbutton.button ==
|
bool rc_ok = rc.isConnected();
|
||||||
2) { // X 버튼: 라이다 장애물 회피 토글
|
rc.getChannel(rc_cfg.ch_steer, raw_steer);
|
||||||
use_lidar = !use_lidar;
|
rc.getChannel(rc_cfg.ch_throttle, raw_throttle);
|
||||||
std::cout << "\n[정보] 라이다 회피 기능: "
|
rc.getChannel(rc_cfg.ch_brake, raw_brake);
|
||||||
<< (use_lidar ? "ON" : "OFF") << std::endl;
|
rc.getChannel(rc_cfg.ch_speed, raw_speed_mode);
|
||||||
} else if (event.jbutton.button == 1 ||
|
|
||||||
event.jbutton.button == 6) { // B or Back
|
bool steer_range_ok =
|
||||||
g_running = 0;
|
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; // 범위 밖 또는 비정상 점프 -> 손상된 프레임으로 간주
|
||||||
}
|
}
|
||||||
|
|
||||||
Sint16 lx_raw = SDL_JoystickGetAxis(joystick, 0);
|
bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min &&
|
||||||
Sint16 ly_raw = SDL_JoystickGetAxis(joystick, 1);
|
raw_throttle <= rc_cfg.throttle_max;
|
||||||
Sint16 rx_raw = SDL_JoystickGetAxis(joystick, 3);
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
float ly = applyDeadzone(-static_cast<float>(ly_raw) / 32767.0f);
|
// CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
|
||||||
float lx = applyDeadzone(static_cast<float>(lx_raw) / 32767.0f);
|
// 직접 눈으로 검증할 수 있도록 HUD/CSV에 그대로 노출).
|
||||||
float rx = applyDeadzone(static_cast<float>(rx_raw) / 32767.0f);
|
int raw_ch[9]; // 인덱스 1..8 사용
|
||||||
|
for (int ch = 1; ch <= 8; ++ch) {
|
||||||
|
if (!rc.getChannel(ch, raw_ch[ch]))
|
||||||
|
raw_ch[ch] = 0; // 아직 수신 못한 채널은 0으로 표시
|
||||||
|
}
|
||||||
|
|
||||||
if (std::abs(lx) < 0.15f)
|
if (!rc_ok && prev_rc_ok) {
|
||||||
lx = 0.0f;
|
std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms
|
||||||
if (std::abs(rx) < 0.15f)
|
<< "ms 이상 미수신) -> 비상 정지.\n";
|
||||||
rx = 0.0f;
|
}
|
||||||
|
prev_rc_ok = rc_ok;
|
||||||
|
|
||||||
float v_x = ly * max_v; // 선속도 m/s
|
// 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리
|
||||||
|
bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold);
|
||||||
|
|
||||||
|
// 3.2 속도 제한 설정 (CH6)
|
||||||
|
float v_max_now = (raw_speed_mode >= rc_cfg.speed_threshold)
|
||||||
|
? rc_cfg.v_max_low
|
||||||
|
: rc_cfg.v_max_high;
|
||||||
|
|
||||||
|
float v_x = 0.0f; // 선속도 m/s
|
||||||
float omega = 0.0f; // 각속도 rad/s
|
float omega = 0.0f; // 각속도 rad/s
|
||||||
|
|
||||||
|
if (!brake_on) {
|
||||||
|
// 3.3 전후진 속도 계산 (CH2) — 실측 결과 raw 값이 높을수록 전진(+),
|
||||||
|
// 낮을수록 후진(-)이라 CH1(조향)과는 반대 부호 방향이다.
|
||||||
|
int t = std::clamp(raw_throttle, rc_cfg.throttle_min, rc_cfg.throttle_max);
|
||||||
|
if (t >= rc_cfg.throttle_center_lo && t <= rc_cfg.throttle_center_hi) {
|
||||||
|
v_x = 0.0f;
|
||||||
|
} else if (t < rc_cfg.throttle_center_lo) {
|
||||||
|
v_x = -(static_cast<float>(rc_cfg.throttle_center_lo - t) /
|
||||||
|
static_cast<float>(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
|
||||||
|
v_max_now;
|
||||||
|
} else {
|
||||||
|
v_x = (static_cast<float>(t - rc_cfg.throttle_center_hi) /
|
||||||
|
static_cast<float>(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) *
|
||||||
|
v_max_now;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3.4 좌우 조향 각속도 계산 (CH1)
|
||||||
|
int s = std::clamp(raw_steer, rc_cfg.steer_min, rc_cfg.steer_max);
|
||||||
|
if (s >= rc_cfg.steer_center_lo && s <= rc_cfg.steer_center_hi) {
|
||||||
|
omega = 0.0f;
|
||||||
|
} else if (s < rc_cfg.steer_center_lo) {
|
||||||
|
omega = -(static_cast<float>(rc_cfg.steer_center_lo - s) /
|
||||||
|
static_cast<float>(rc_cfg.steer_center_lo - rc_cfg.steer_min)) *
|
||||||
|
omega_max;
|
||||||
|
} else {
|
||||||
|
omega = (static_cast<float>(s - rc_cfg.steer_center_hi) /
|
||||||
|
static_cast<float>(rc_cfg.steer_max - rc_cfg.steer_center_hi)) *
|
||||||
|
omega_max;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// ---------------------------------------------------------------------
|
// ---------------------------------------------------------------------
|
||||||
// LiDAR Obstacle Avoidance & Stopping Logic Integration
|
// LiDAR Obstacle Avoidance & Stopping Logic Integration
|
||||||
// (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작)
|
// (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작)
|
||||||
@@ -1011,8 +1433,8 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 2. Side Dodge Steering (전진 중에만 동작)
|
// 2. Side Dodge Steering (전진 중, 사용자 조향 입력이 없을 때만 동작)
|
||||||
if (v_x > 0.01f && rx == 0.0f) {
|
if (v_x > 0.01f && omega == 0.0f) {
|
||||||
float right_intensity = 0.0f;
|
float right_intensity = 0.0f;
|
||||||
float left_intensity = 0.0f;
|
float left_intensity = 0.0f;
|
||||||
|
|
||||||
@@ -1054,17 +1476,18 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 제자리 회전 여부: 왼쪽 스틱 전후(ly) 입력이 완전히 중립일 때만 진입한다.
|
// 제자리 회전 여부: CH2(전후진) 입력이 완전히 중립(v_x == 0.0f)이면서
|
||||||
// 그렇지 않으면(주행 중 오른쪽 스틱이 드리프트 등으로 미세하게라도 감지될
|
// CH1(조향) 입력이 있을 때만 진입한다. v_x는 데드존 처리 시 정확히
|
||||||
// 경우) 전진 입력이 통째로 무시되고 제자리 회전 로직으로 튀어버려, 커브
|
// 0.0f로 설정되므로 등호 비교가 안전하다.
|
||||||
// 선회 중에 갑자기 이상하게 회전하려는 문제가 생긴다. ly는 applyDeadzone을
|
bool is_spin_turn = (v_x == 0.0f) && (std::abs(omega) > 0.0f);
|
||||||
// 거쳐 데드존 이하일 때 정확히 0.0f를 반환하므로 등호 비교가 안전하다.
|
|
||||||
bool is_spin_turn = std::abs(rx) > 0.0f && ly == 0.0f;
|
|
||||||
|
|
||||||
if (is_spin_turn) {
|
if (is_spin_turn) {
|
||||||
// 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합
|
// 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합 (스크럽
|
||||||
omega = -rx * (max_spin_v / (effective_w / 2.0f));
|
// 마찰 저항 감소를 위해 ICR을 로봇 중심에서 살짝 벗어나게 함)
|
||||||
float v_bias = std::abs(rx) * max_spin_v * spin_arc_bias;
|
float norm = (omega_max > 0.0f)
|
||||||
|
? std::clamp(std::abs(omega) / omega_max, 0.0f, 1.0f)
|
||||||
|
: 0.0f;
|
||||||
|
float v_bias = norm * max_spin_v * spin_arc_bias;
|
||||||
float v_l = v_bias - omega * (effective_w / 2.0f);
|
float v_l = v_bias - omega * (effective_w / 2.0f);
|
||||||
float v_r = v_bias + omega * (effective_w / 2.0f);
|
float v_r = v_bias + omega * (effective_w / 2.0f);
|
||||||
|
|
||||||
@@ -1073,33 +1496,13 @@ 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 {
|
||||||
// 직진 및 차동 선회 Kinematics
|
// 직진 및 차동 선회 Kinematics (CH1 조향 + 라이다 회피 오프셋 포함)
|
||||||
if (std::abs(rx) == 0.0f && std::abs(lx) == 0.0f &&
|
|
||||||
std::abs(omega) > 0.0f) {
|
|
||||||
// Automated avoidance steering active
|
|
||||||
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;
|
||||||
target_fl = v_l * rpm_per_ms;
|
target_fl = v_l * rpm_per_ms;
|
||||||
target_fr = -v_r * rpm_per_ms;
|
target_fr = -v_r * rpm_per_ms;
|
||||||
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 {
|
|
||||||
// 선회 강도(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_r = v_x + omega * k_skid;
|
|
||||||
|
|
||||||
target_fl = v_l * rpm_per_ms;
|
|
||||||
target_fr = -v_r * rpm_per_ms;
|
|
||||||
target_rl = v_l * rpm_per_ms;
|
|
||||||
target_rr = -v_r * rpm_per_ms;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 0으로 낮춰
|
// 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 0으로 낮춰
|
||||||
@@ -1114,12 +1517,33 @@ int main(int argc, char **argv) {
|
|||||||
if (airborne_rr)
|
if (airborne_rr)
|
||||||
target_rr = 0.0f;
|
target_rr = 0.0f;
|
||||||
|
|
||||||
if (state == "STOPPED") {
|
// 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시
|
||||||
if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
|
// 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;
|
||||||
|
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);
|
||||||
|
state = "STOPPED";
|
||||||
|
} else if (!brake_on && prev_brake_on) {
|
||||||
driver_front.setBrakes(false);
|
driver_front.setBrakes(false);
|
||||||
if (driver_rear_ptr)
|
if (driver_rear_ptr)
|
||||||
driver_rear_ptr->setBrakes(false);
|
driver_rear_ptr->setBrakes(false);
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
}
|
||||||
|
prev_brake_on = brake_on;
|
||||||
|
|
||||||
|
if (brake_on) {
|
||||||
|
// 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) {
|
||||||
state = "RUNNING";
|
state = "RUNNING";
|
||||||
}
|
}
|
||||||
} else if (state == "RUNNING") {
|
} else if (state == "RUNNING") {
|
||||||
@@ -1147,14 +1571,10 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
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, decel_rate, jerk_now);
|
||||||
decel_rate, jerk_now);
|
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now);
|
||||||
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, accel_rate,
|
cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now);
|
||||||
decel_rate, jerk_now);
|
cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, 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);
|
|
||||||
|
|
||||||
if (std::abs(target_fl) < 0.1f && std::abs(target_fr) < 0.1f &&
|
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) {
|
std::abs(cmd_fl) < 0.5f && std::abs(cmd_fr) < 0.5f) {
|
||||||
@@ -1162,14 +1582,10 @@ int main(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
} else if (state == "STOPPING") {
|
} else if (state == "STOPPING") {
|
||||||
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, decel_rate,
|
cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate, jerk_now);
|
||||||
decel_rate, jerk_now);
|
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now);
|
||||||
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate,
|
cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now);
|
||||||
decel_rate, jerk_now);
|
cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, 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);
|
|
||||||
|
|
||||||
if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) {
|
if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) {
|
||||||
cmd_fl = 0.0f;
|
cmd_fl = 0.0f;
|
||||||
@@ -1180,10 +1596,8 @@ int main(int argc, char **argv) {
|
|||||||
driver_front.setRPMs(0, 0);
|
driver_front.setRPMs(0, 0);
|
||||||
if (driver_rear_ptr)
|
if (driver_rear_ptr)
|
||||||
driver_rear_ptr->setRPMs(0, 0);
|
driver_rear_ptr->setRPMs(0, 0);
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
// 스로틀 중립 복귀로 인한 자연 정지: 전자식 브레이크는 잠그지 않는다
|
||||||
driver_front.setBrakes(true);
|
// (CH5를 눌렀을 때만 잠금 — 위 브레이크 최우선 로직 참고).
|
||||||
if (driver_rear_ptr)
|
|
||||||
driver_rear_ptr->setBrakes(true);
|
|
||||||
state = "STOPPED";
|
state = "STOPPED";
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1333,8 +1747,13 @@ int main(int argc, char **argv) {
|
|||||||
float t_ms = std::chrono::duration<float, std::milli>(
|
float t_ms = std::chrono::duration<float, std::milli>(
|
||||||
std::chrono::steady_clock::now() - log_start_time)
|
std::chrono::steady_clock::now() - log_start_time)
|
||||||
.count();
|
.count();
|
||||||
log_stream << t_ms << ',' << state << ',' << ly << ',' << lx << ','
|
log_stream << t_ms << ',' << state << ',' << raw_steer << ','
|
||||||
<< rx << ',' << cmd_fl << ',' << cmd_fr << ',' << cmd_rl
|
<< raw_throttle << ',' << (rc_ok ? 1 : 0) << ','
|
||||||
|
<< (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ','
|
||||||
|
<< omega << ',' << raw_ch[1] << ',' << raw_ch[2] << ','
|
||||||
|
<< raw_ch[3] << ',' << raw_ch[4] << ',' << raw_ch[5] << ','
|
||||||
|
<< raw_ch[6] << ',' << raw_ch[7] << ',' << raw_ch[8] << ','
|
||||||
|
<< cmd_fl << ',' << cmd_fr << ',' << cmd_rl
|
||||||
<< ',' << cmd_rr << ',' << fl_fb << ',' << fr_fb << ','
|
<< ',' << cmd_rr << ',' << fl_fb << ',' << fr_fb << ','
|
||||||
<< rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp
|
<< rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp
|
||||||
<< ',' << rl_amp << ',' << rr_amp << ',' << wheel_status_str[0]
|
<< ',' << rl_amp << ',' << rr_amp << ',' << wheel_status_str[0]
|
||||||
@@ -1344,9 +1763,20 @@ int main(int argc, char **argv) {
|
|||||||
<< dist_axle << ',' << comm_ms << '\n';
|
<< dist_axle << ',' << comm_ms << '\n';
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("\r\033[K[%-8s] AXLE:%5.3fm | %-28s | I(A):FL%+4.1f FR%+4.1f "
|
// RC 조종기 입력값 피드백: 실기 조작 중 CH1~CH8 raw 값(CH5=브레이크 값
|
||||||
"RL%+4.1f RR%+4.1f | WHL(FL/FR/RL/RR):%s%s | %4.1fms",
|
// 직접 확인용 포함)과 그로부터 계산된 선속도/각속도 지령을 실시간으로
|
||||||
state.c_str(), dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
|
// 확인할 수 있도록 HUD에 표시한다.
|
||||||
|
const char *speed_mode_str =
|
||||||
|
(v_max_now <= rc_cfg.v_max_low + 0.001f) ? "LOW " : "HIGH";
|
||||||
|
printf("\r\033[K[%-8s] RC:%s%s "
|
||||||
|
"C1:%4d C2:%4d C3:%4d C4:%4d C5:%4d C6:%4d C7:%4d C8:%4d "
|
||||||
|
"SPD:%s VX:%+4.2f OMG:%+4.2f | AXLE:%5.3fm | %-28s | "
|
||||||
|
"I(A):FL%+4.1f FR%+4.1f RL%+4.1f RR%+4.1f | "
|
||||||
|
"WHL(FL/FR/RL/RR):%s%s | %4.1fms",
|
||||||
|
state.c_str(), rc_ok ? "OK" : "LOST", brake_on ? "[BRK]" : " ",
|
||||||
|
raw_ch[1], raw_ch[2], raw_ch[3], raw_ch[4], raw_ch[5], raw_ch[6],
|
||||||
|
raw_ch[7], raw_ch[8], speed_mode_str, v_x, omega,
|
||||||
|
dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
|
||||||
rl_amp, rr_amp, wheel_status_str,
|
rl_amp, rr_amp, wheel_status_str,
|
||||||
fault_active ? " [!!알람!!]" : "", comm_ms);
|
fault_active ? " [!!알람!!]" : "", comm_ms);
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
@@ -1373,9 +1803,7 @@ int main(int argc, char **argv) {
|
|||||||
std::cout << "[정보] CSV 로그 저장 완료: " << log_path << "\n";
|
std::cout << "[정보] CSV 로그 저장 완료: " << log_path << "\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
if (joystick)
|
rc.stop();
|
||||||
SDL_JoystickClose(joystick);
|
|
||||||
SDL_Quit();
|
|
||||||
|
|
||||||
std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
|
std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
Binary file not shown.
Reference in New Issue
Block a user