azmo/fori_zltech_motor_test
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add doc

Browse Source
This commit is contained in:
robin
2026-08-19 13:56:22 +09:00
parent bb5a0adc0c
commit 6be30eb929
4 changed files with 263 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/joystick.md
+23 -17
View File
@@ -6,6 +6,12 @@
* 하드웨어: USB to TTL 컨버터 (`/dev/ttyUSB*` 또는 COM 포트) * 하드웨어: USB to TTL 컨버터 (`/dev/ttyUSB*` 또는 COM 포트)
* 수신기: XR1 V1.0 * 수신기: XR1 V1.0
* 조종기: RadioMaster Pocket * 조종기: 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 프레이밍만 신뢰함.)
--- ---
@@ -15,10 +21,10 @@
| 채널 | 입력 장치 | Raw Value 범위 | 할당 기능 | 동작 설명 | | 채널 | 입력 장치 | Raw Value 범위 | 할당 기능 | 동작 설명 |
| :--- | :--- | :--- | :--- | :--- | | :--- | :--- | :--- | :--- | :--- |
| **CH1** | 롤/조향 스틱 | • `1001`: 좌측 끝<br>• `1480 ~ 1500`: 중앙 (데드존)<br>• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1480~1500`) 적용<br>• 좌회전(-) / 우회전(+) 정규화 | | **CH1** | 롤/조향 스틱 | • `1005`: 좌측 끝<br>• `1481 ~ 1503`: 중앙 (데드존)<br>• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1481~1503`) 적용<br>• 좌회전(-) / 우회전(+) 정규화 |
| **CH2** | 피치/스로틀 스틱 | • `1000`: 위 끝 (전진)<br>• `1500`: 중앙 (정지)<br>• `2000`: 아래 끝 (후진) | **전후진 (Throttle / Linear X)** | • `1000` 방향으로 이동 시 전진<br>• `2000` 방향으로 이동 시 후진<br>• CH7에서 결정된 최대 속도 기준으로 선형 매핑 | | **CH2** | 피치/스로틀 스틱 | • `1005`: 최대 후진<br>• `1500`: 중앙 (정지)<br>• `2000`: 최대 전진 | **전후진 (Throttle / Linear X)** | • 값이 높을수록 전진, 낮을수록 후진 (CH1과 반대 방향)<br>• CH6에서 결정된 최대 속도 기준으로 선형 매핑 |
| **CH5** | 2단 스위치 | • `1011`: 미작동 (OFF)<br>• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)<br>• `1011` 수신 시 일반 주행 허용 | | **CH5** | 2단 스위치 | • `1012`: 미작동 (OFF)<br>• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)<br>• `1012` 수신 시 일반 주행 허용 |
| **CH7** | 2단 스위치 | • `1988`: 위쪽 누름<br>• `1011`: 아래쪽 누름 | **속도 모드 선택 (Speed Mode)** | • `1988` (위쪽): 저속 모드 (최대 **0.3 m/s**)<br>• `1011` (아래쪽): 고속 모드 (최대 **0.6 m/s**) | | **CH6** | 2단 스위치 | • `1832`: 저속 위치<br>• `1012~1090`: 고속 위치 | **속도 모드 선택 (Speed Mode)** | • `1832`(≥1500 부근): 저속 모드 (최대 **0.3 m/s**)<br>• `1012~1090`(<1500): 고속 모드 (최대 **1.5 m/s**) |
--- ---
@@ -27,25 +33,25 @@
### 3.1 브레이크 최우선 로직 (CH5) ### 3.1 브레이크 최우선 로직 (CH5)
* $\text{CH5} \ge 1500$ (누름 / `1988` 부근): * $\text{CH5} \ge 1500$ (누름 / `1988` 부근):
$$\text{Linear Velocity} = 0.0, \quad \text{Angular Velocity} = 0.0$$ $$\text{Linear Velocity} = 0.0, \quad \text{Angular Velocity} = 0.0$$
* $\text{CH5} < 1500$ (안누름 / `1011` 부근): 정상 주행 로직 실행 * $\text{CH5} < 1500$ (안누름 / `1012` 부근): 정상 주행 로직 실행
### 3.2 속도 제한 설정 (CH7) ### 3.2 속도 제한 설정 (CH6)
* $\text{CH7} \ge 1500$ (위쪽 / `1988` 부근): $V_{\max} = 0.3 \text{ m/s}$ (저속 모드) * $\text{CH6} \ge 1500$ (`1832` 부근): $V_{\max} = 0.3 \text{ m/s}$ (저속 모드)
* $\text{CH7} < 1500$ (아래쪽 / `1011` 부근): $V_{\max} = 0.6 \text{ m/s}$ (고속 모드) * $\text{CH6} < 1500$ (`1012~1090` 부근): $V_{\max} = 1.5 \text{ m/s}$ (고속 모드)
### 3.3 전후진 속도 계산 (CH2) ### 3.3 전후진 속도 계산 (CH2)
* **데드존(Deadband):** $1480 \le \text{CH2} \le 1520 \rightarrow V_x = 0.0$ * **데드존(Deadband):** $1480 \le \text{CH2} \le 1520 \rightarrow V_x = 0.0$
* **전진 구간 ($1000 \le \text{CH2} < 1480$):** * **후진 구간 ($1005 \le \text{CH2} < 1480$):**
$$V_x = \left( \frac{1480 - \text{CH2}}{1480 - 1000} \right) \times V_{\max}$$ $$V_x = -\left( \frac{1480 - \text{CH2}}{1480 - 1005} \right) \times V_{\max}$$
* **후진 구간 ($1520 < \text{CH2} \le 2000$):** * **전진 구간 ($1520 < \text{CH2} \le 2000$):**
$$V_x = -\left( \frac{\text{CH2} - 1520}{2000 - 1520} \right) \times V_{\max}$$ $$V_x = \left( \frac{\text{CH2} - 1520}{2000 - 1520} \right) \times V_{\max}$$
### 3.4 좌우 조향 각속도 계산 (CH1) ### 3.4 좌우 조향 각속도 계산 (CH1)
* **데드존(Deadband):** $1480 \le \text{CH1} \le 1500 \rightarrow \omega_z = 0.0$ * **데드존(Deadband):** $1481 \le \text{CH1} \le 1503 \rightarrow \omega_z = 0.0$
* **좌회전 구간 ($1001 \le \text{CH1} < 1480$):** * **좌회전 구간 ($1005 \le \text{CH1} < 1481$):**
$$\omega_z = -\left( \frac{1480 - \text{CH1}}{1480 - 1001} \right) \times \Omega_{\max}$$ $$\omega_z = -\left( \frac{1481 - \text{CH1}}{1481 - 1005} \right) \times \Omega_{\max}$$
* **우회전 구간 ($1500 < \text{CH1} \le 2000$):** * **우회전 구간 ($1503 < \text{CH1} \le 2000$):**
$$\omega_z = +\left( \frac{\text{CH1} - 1500}{2000 - 1500} \right) \times \Omega_{\max}$$ $$\omega_z = +\left( \frac{\text{CH1} - 1503}{2000 - 1503} \right) \times \Omega_{\max}$$
--- ---
mac/mac_code
+79
View File
@@ -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()
xbox_motor_control.cpp
+158 -84
View File
@@ -604,28 +604,50 @@ LidarObstacleDetector *LidarObstacleDetector::instance_ = nullptr;
// ----------------------------------------------------------------------------- // -----------------------------------------------------------------------------
// RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈 // RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈
// doc/joystick.md 기능 명세서 기반: 텍스트 라인 "CH1:1500,CH2:1500,..." 포맷을 // 실기 캡처로 확인한 실제 프로토콜: CRSF(Crossfire), 420000bps, 8N1.
// 파싱해 채널별 raw 값(1000~2000)을 제공한다. // 프레임: [0]=Sync(0xC8) [1]=Length [2]=FrameType [3..]=Payload [-1]=CRC8
// FrameType 0x16(RC_CHANNELS_PACKED)의 22바이트 payload에 16채널이 11비트씩
// 리틀엔디안으로 패킹되어 있다 (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 { struct RcConfig {
std::string port = "/dev/ttyUSB1"; std::string port = "/dev/ttyUSB1";
int baud = 115200; int baud = 420000; // CRSF 표준 보드레이트
int ch_steer = 1; // CH1: 좌우 조향 int ch_steer = 1; // CH1: 좌우 조향
int ch_throttle = 2; // CH2: 전후진 int ch_throttle = 2; // CH2: 전후진
int ch_brake = 5; // CH5: 비상 브레이크 int ch_brake = 5; // CH5: 비상 브레이크
int ch_speed = 7; // CH7: 속도 모드 int ch_speed = 6; // CH6: 속도 모드
int steer_min = 1001, steer_center_lo = 1480, steer_center_hi = 1500, // 실기 캡처로 확인한 실측값 (좌 1005 ~ 중 1481~1503 ~ 우 2000)
int steer_min = 1005, steer_center_lo = 1481, steer_center_hi = 1503,
steer_max = 2000; steer_max = 2000;
int throttle_min = 1000, throttle_center_lo = 1480, throttle_center_hi = 1520, // 실측값: 후진끝 1005 ~ 중 1500 ~ 전진끝 2000 (CH2는 값이 높을수록 전진 —
// CH1과 반대 방향이라 아래 3.3 매핑 로직도 그에 맞춰 부호가 반대다)
int throttle_min = 1005, throttle_center_lo = 1480, throttle_center_hi = 1520,
throttle_max = 2000; throttle_max = 2000;
int brake_threshold = 1500; // 이상이면 브레이크 ON int brake_threshold = 1500; // 이상이면 브레이크 ON
int speed_threshold = 1500; // 이상이면 저속 모드 int speed_threshold = 1500; // 이상이면 저속 모드
float v_max_low = 0.3f; // m/s (CH7 저속 모드) float v_max_low = 0.3f; // m/s (CH6 저속 모드)
float v_max_high = 0.6f; // m/s (CH7 고속 모드) float v_max_high = 1.5f; // m/s (CH6 고속 모드)
int failsafe_timeout_ms = 200; // 이 시간 이상 신호 미수신 시 비상정지 int failsafe_timeout_ms = 200; // 이 시간 이상 신호 미수신 시 비상정지
}; };
@@ -679,7 +701,11 @@ private:
std::chrono::steady_clock::time_point last_rx_time_{}; std::chrono::steady_clock::time_point last_rx_time_{};
bool port_open_ = false; bool port_open_ = false;
std::string line_buf_; std::vector<uint8_t> frame_buf_;
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() { bool openPort() {
fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
@@ -688,46 +714,40 @@ private:
fcntl(fd_, F_SETFL, O_NONBLOCK); fcntl(fd_, F_SETFL, O_NONBLOCK);
struct termios options; // CRSF 표준 420000bps는 termios의 표준 B-상수 목록에 없어 TCSETS2 +
tcgetattr(fd_, &options); // BOTHER + c_ispeed/c_ospeed(임의 정수 보드레이트) ioctl로 설정한다.
struct termios2 tio;
speed_t speed = B115200; if (ioctl(fd_, TCGETS2, &tio) < 0) {
switch (cfg_.baud) { close(fd_);
case 9600: fd_ = -1;
speed = B9600; return false;
break;
case 19200:
speed = B19200;
break;
case 38400:
speed = B38400;
break;
case 57600:
speed = B57600;
break;
default:
speed = B115200;
break;
} }
cfsetispeed(&options, speed); tio.c_cflag &= ~CBAUD;
cfsetospeed(&options, speed); tio.c_cflag |= BOTHER;
tio.c_ispeed = static_cast<speed_t>(cfg_.baud);
tio.c_ospeed = static_cast<speed_t>(cfg_.baud);
options.c_cflag &= ~PARENB; tio.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB; tio.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE; tio.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; tio.c_cflag |= CS8;
options.c_cflag &= ~CRTSCTS; tio.c_cflag &= ~CRTSCTS;
options.c_cflag |= CREAD | CLOCAL; tio.c_cflag |= CREAD | CLOCAL;
// 텍스트 라인 프로토콜이므로 canonical 모드(라인 단위)로 사용 // 바이너리 프로토콜이므로 raw 모드(비정규/논캐노니컬)로 읽는다.
options.c_lflag |= ICANON; tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | IEXTEN);
options.c_lflag &= ~(ECHO | ECHOE | ISIG); tio.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
ISTRIP | INLCR | IGNCR | ICRNL); ISTRIP | INLCR | IGNCR | ICRNL);
options.c_oflag &= ~OPOST; tio.c_oflag &= ~OPOST;
tio.c_cc[VMIN] = 0;
tio.c_cc[VTIME] = 0;
tcsetattr(fd_, TCSANOW, &options); if (ioctl(fd_, TCSETS2, &tio) < 0) {
close(fd_);
fd_ = -1;
return false;
}
tcflush(fd_, TCIOFLUSH); tcflush(fd_, TCIOFLUSH);
return true; return true;
} }
@@ -741,37 +761,63 @@ private:
port_open_ = false; port_open_ = false;
} }
void parseLine(const std::string &line) { // 축적된 바이트 버퍼에서 CRSF 프레임을 최대한 뽑아낸다. Sync 바이트를
// 찾고, 선언된 Length만큼 버퍼에 데이터가 쌓일 때까지 기다린 뒤
// RC_CHANNELS_PACKED(0x16) 프레임이면 16채널(11비트씩)을 언패킹한다.
// (CRC8은 이 송신기에서 표준 DVB-S2 값과 맞지 않아 검증하지 않음 — 실기
// 캡처로 확인된 mac/mac_code 레퍼런스와 동일한 방식.)
void parseCrsfBuffer() {
std::map<int, int> parsed; std::map<int, int> parsed;
size_t pos = 0; size_t i = 0;
while (pos <= line.size()) { while (i < frame_buf_.size()) {
size_t comma = line.find(',', pos); if (frame_buf_[i] != CRSF_SYNC) {
std::string tok = line.substr( ++i;
pos, comma == std::string::npos ? std::string::npos : comma - pos); continue;
}
if (i + 1 >= frame_buf_.size())
break; // length 바이트 대기
size_t b = tok.find_first_not_of(" \t\r\n"); uint8_t length = frame_buf_[i + 1];
size_t e = tok.find_last_not_of(" \t\r\n"); size_t total_len = static_cast<size_t>(length) + 2;
if (b != std::string::npos) { if (length < 2 || total_len > CRSF_MAX_FRAME_LEN) {
tok = tok.substr(b, e - b + 1); // Sync처럼 보이는 노이즈 바이트: 1바이트만 버리고 재동기화
if (tok.size() >= 4 && (tok[0] == 'C' || tok[0] == 'c') && ++i;
(tok[1] == 'H' || tok[1] == 'h')) { continue;
size_t colon = tok.find(':'); }
if (colon != std::string::npos) { if (i + total_len > frame_buf_.size())
try { break; // 프레임 전체 수신 대기
int ch_num = std::stoi(tok.substr(2, colon - 2));
int val = std::stoi(tok.substr(colon + 1)); uint8_t frame_type = frame_buf_[i + 2];
parsed[ch_num] = val; if (frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED && length >= 23) {
} catch (...) { // 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));
parsed[ch + 1] = us_val; // CH1..CH16 (1-based)
} }
} }
i += total_len;
} }
if (comma == std::string::npos) // 처리하지 못한 잔여 바이트(다음 프레임 일부)만 남기고 버퍼 정리
break; frame_buf_.erase(frame_buf_.begin(), frame_buf_.begin() + i);
pos = comma + 1; if (frame_buf_.size() > CRSF_MAX_FRAME_LEN * 4) // 비정상 누적 방지
} frame_buf_.clear();
if (!parsed.empty()) { if (!parsed.empty()) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
@@ -812,15 +858,8 @@ private:
ssize_t n = read(fd_, buf, sizeof(buf)); ssize_t n = read(fd_, buf, sizeof(buf));
if (n > 0) { if (n > 0) {
line_buf_.append(buf, static_cast<size_t>(n)); frame_buf_.insert(frame_buf_.end(), buf, buf + n);
size_t nl; parseCrsfBuffer();
while ((nl = line_buf_.find('\n')) != std::string::npos) {
std::string line = line_buf_.substr(0, nl);
line_buf_.erase(0, nl + 1);
parseLine(line);
}
if (line_buf_.size() > 4096) // 개행 없는 쓰레기 데이터 누적 방지
line_buf_.clear();
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) { } else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n"; std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n";
closePort(); closePort();
@@ -1052,6 +1091,7 @@ int main(int argc, char **argv) {
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,rc_steer,rc_throttle,rc_ok,brake,vmax,vx,omega," 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,"
@@ -1077,6 +1117,18 @@ int main(int argc, char **argv) {
} }
} }
// 모터(--port)와 RC 수신기(--rc_port)가 같은 물리 포트를 가리키면 두
// 스레드가 같은 장치의 read()를 두고 경쟁해 RC 라인 파싱이 깨진다.
// 흔한 실수(예: 모터 미연결 상태에서 RC 수신기가 ttyUSB0에 꽂혀있는데
// --rc_port를 기본값 ttyUSB1로 둔 채 실행)를 바로 알아차리도록 경고한다.
if (port1 == rc_cfg.port) {
std::cout << "\n[경고] --port(모터, " << port1 << ")와 --rc_port(RC 수신기, "
<< rc_cfg.port
<< ")가 동일한 포트로 설정되어 있습니다. 두 기능이 같은 장치를 "
"동시에 읽으면 RC 데이터 파싱이 깨질 수 있으니 서로 다른 "
"포트로 지정하세요.\n\n";
}
RcReceiver rc(rc_cfg); RcReceiver rc(rc_cfg);
rc.start(); rc.start();
std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 (" std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 ("
@@ -1221,6 +1273,14 @@ int main(int argc, char **argv) {
rc.getChannel(rc_cfg.ch_brake, raw_brake); rc.getChannel(rc_cfg.ch_brake, raw_brake);
rc.getChannel(rc_cfg.ch_speed, raw_speed_mode); rc.getChannel(rc_cfg.ch_speed, raw_speed_mode);
// CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
// 직접 눈으로 검증할 수 있도록 HUD/CSV에 그대로 노출).
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 (!rc_ok && prev_rc_ok) { if (!rc_ok && prev_rc_ok) {
std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms
<< "ms 이상 미수신) -> 비상 정지.\n"; << "ms 이상 미수신) -> 비상 정지.\n";
@@ -1230,7 +1290,7 @@ int main(int argc, char **argv) {
// 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리 // 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리
bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold); bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold);
// 3.2 속도 제한 설정 (CH7) // 3.2 속도 제한 설정 (CH6)
float v_max_now = (raw_speed_mode >= rc_cfg.speed_threshold) float v_max_now = (raw_speed_mode >= rc_cfg.speed_threshold)
? rc_cfg.v_max_low ? rc_cfg.v_max_low
: rc_cfg.v_max_high; : rc_cfg.v_max_high;
@@ -1239,16 +1299,17 @@ int main(int argc, char **argv) {
float omega = 0.0f; // 각속도 rad/s float omega = 0.0f; // 각속도 rad/s
if (!brake_on) { if (!brake_on) {
// 3.3 전후진 속도 계산 (CH2) // 3.3 전후진 속도 계산 (CH2) — 실측 결과 raw 값이 높을수록 전진(+),
// 낮을수록 후진(-)이라 CH1(조향)과는 반대 부호 방향이다.
int t = std::clamp(raw_throttle, rc_cfg.throttle_min, rc_cfg.throttle_max); 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) { if (t >= rc_cfg.throttle_center_lo && t <= rc_cfg.throttle_center_hi) {
v_x = 0.0f; v_x = 0.0f;
} else if (t < rc_cfg.throttle_center_lo) { } else if (t < rc_cfg.throttle_center_lo) {
v_x = (static_cast<float>(rc_cfg.throttle_center_lo - t) / v_x = -(static_cast<float>(rc_cfg.throttle_center_lo - t) /
static_cast<float>(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) * static_cast<float>(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
v_max_now; v_max_now;
} else { } else {
v_x = -(static_cast<float>(t - rc_cfg.throttle_center_hi) / v_x = (static_cast<float>(t - rc_cfg.throttle_center_hi) /
static_cast<float>(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) * static_cast<float>(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) *
v_max_now; v_max_now;
} }
@@ -1619,7 +1680,10 @@ int main(int argc, char **argv) {
log_stream << t_ms << ',' << state << ',' << raw_steer << ',' log_stream << t_ms << ',' << state << ',' << raw_steer << ','
<< raw_throttle << ',' << (rc_ok ? 1 : 0) << ',' << raw_throttle << ',' << (rc_ok ? 1 : 0) << ','
<< (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ',' << (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ','
<< omega << ',' << cmd_fl << ',' << cmd_fr << ',' << cmd_rl << 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]
@@ -1629,9 +1693,19 @@ int main(int argc, char **argv) {
<< dist_axle << ',' << comm_ms << '\n'; << dist_axle << ',' << comm_ms << '\n';
} }
printf("\r\033[K[%-8s] RC:%s%s 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", // 직접 확인용 포함)과 그로부터 계산된 선속도/각속도 지령을 실시간으로
// 확인할 수 있도록 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]" : " ", 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, 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);
xbox_motor_control_cpp
BIN
View File
Binary file not shown.