diff --git a/Makefile b/Makefile index c934c91..c4c573b 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ CXX = g++ 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 SRC = xbox_motor_control.cpp diff --git a/doc/joystick.md b/doc/joystick.md new file mode 100644 index 0000000..adf81cf --- /dev/null +++ b/doc/joystick.md @@ -0,0 +1,61 @@ +# [기능 명세서] 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 + +--- + +## 2. 하드웨어 및 채널 매핑 사양 + +### 2.1 채널별 입력 범위 및 기능 정의 + +| 채널 | 입력 장치 | Raw Value 범위 | 할당 기능 | 동작 설명 | +| :--- | :--- | :--- | :--- | :--- | +| **CH1** | 롤/조향 스틱 | • `1001`: 좌측 끝
• `1480 ~ 1500`: 중앙 (데드존)
• `2000`: 우측 끝 | **좌우 조향 (Steering / Angular Z)** | • 중앙 데드존(`1480~1500`) 적용
• 좌회전(-) / 우회전(+) 정규화 | +| **CH2** | 피치/스로틀 스틱 | • `1000`: 위 끝 (전진)
• `1500`: 중앙 (정지)
• `2000`: 아래 끝 (후진) | **전후진 (Throttle / Linear X)** | • `1000` 방향으로 이동 시 전진
• `2000` 방향으로 이동 시 후진
• CH7에서 결정된 최대 속도 기준으로 선형 매핑 | +| **CH5** | 2단 스위치 | • `1011`: 미작동 (OFF)
• `1988`: 눌림 (ON) | **전자/비상 브레이크 (Brake)** | • `1988` 수신 시 모든 주행 속도 즉시 0 (`Stop`)
• `1011` 수신 시 일반 주행 허용 | +| **CH7** | 2단 스위치 | • `1988`: 위쪽 누름
• `1011`: 아래쪽 누름 | **속도 모드 선택 (Speed Mode)** | • `1988` (위쪽): 저속 모드 (최대 **0.3 m/s**)
• `1011` (아래쪽): 고속 모드 (최대 **0.6 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$ (안누름 / `1011` 부근): 정상 주행 로직 실행 + +### 3.2 속도 제한 설정 (CH7) +* $\text{CH7} \ge 1500$ (위쪽 / `1988` 부근): $V_{\max} = 0.3 \text{ m/s}$ (저속 모드) +* $\text{CH7} < 1500$ (아래쪽 / `1011` 부근): $V_{\max} = 0.6 \text{ m/s}$ (고속 모드) + +### 3.3 전후진 속도 계산 (CH2) +* **데드존(Deadband):** $1480 \le \text{CH2} \le 1520 \rightarrow V_x = 0.0$ +* **전진 구간 ($1000 \le \text{CH2} < 1480$):** + $$V_x = \left( \frac{1480 - \text{CH2}}{1480 - 1000} \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):** $1480 \le \text{CH1} \le 1500 \rightarrow \omega_z = 0.0$ +* **좌회전 구간 ($1001 \le \text{CH1} < 1480$):** + $$\omega_z = -\left( \frac{1480 - \text{CH1}}{1480 - 1001} \right) \times \Omega_{\max}$$ +* **우회전 구간 ($1500 < \text{CH1} \le 2000$):** + $$\omega_z = +\left( \frac{\text{CH1} - 1500}{2000 - 1500} \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 파라미터 등)으로 관리 가능하도록 모듈화 \ No newline at end of file diff --git a/run_4wd.sh b/run_4wd.sh index bb49520..d706818 100755 --- a/run_4wd.sh +++ b/run_4wd.sh @@ -23,9 +23,9 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then fi echo "==================================================================" -echo " [시작] C++ 100Hz 초저지연 4WD 조이스틱 + 라이다 장애물 회피 시작 ($PORT)" -echo " - 라이다 기능 토글: Xbox 컨트롤러 X 버튼" -echo " - 정지/종료: B 버튼 또는 Ctrl+C" +echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작 ($PORT)" +echo " - 라이다 기능 비활성화: --no_lidar 옵션" +echo " - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C" echo "==================================================================" # 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능) diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp index a76b5cc..7a38ade 100644 --- a/xbox_motor_control.cpp +++ b/xbox_motor_control.cpp @@ -1,12 +1,13 @@ -#include #include #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -601,13 +602,234 @@ public: LidarObstacleDetector *LidarObstacleDetector::instance_ = nullptr; -float applyDeadzone(float val, float deadzone = 0.1f) { - if (std::abs(val) < deadzone) - return 0.0f; - float sign = (val > 0.0f) ? 1.0f : -1.0f; - float norm = (std::abs(val) - deadzone) / (1.0f - deadzone); - return sign * norm; -} +// ----------------------------------------------------------------------------- +// RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈 +// doc/joystick.md 기능 명세서 기반: 텍스트 라인 "CH1:1500,CH2:1500,..." 포맷을 +// 파싱해 채널별 raw 값(1000~2000)을 제공한다. +// ----------------------------------------------------------------------------- +struct RcConfig { + std::string port = "/dev/ttyUSB1"; + int baud = 115200; + + int ch_steer = 1; // CH1: 좌우 조향 + int ch_throttle = 2; // CH2: 전후진 + int ch_brake = 5; // CH5: 비상 브레이크 + int ch_speed = 7; // CH7: 속도 모드 + + int steer_min = 1001, steer_center_lo = 1480, steer_center_hi = 1500, + steer_max = 2000; + int throttle_min = 1000, throttle_center_lo = 1480, throttle_center_hi = 1520, + throttle_max = 2000; + + int brake_threshold = 1500; // 이상이면 브레이크 ON + int speed_threshold = 1500; // 이상이면 저속 모드 + + float v_max_low = 0.3f; // m/s (CH7 저속 모드) + float v_max_high = 0.6f; // m/s (CH7 고속 모드) + + 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 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 lock(mutex_); + if (!port_open_ || channels_.empty()) + return false; + auto now = std::chrono::steady_clock::now(); + return std::chrono::duration_cast( + now - last_rx_time_) + .count() <= cfg_.failsafe_timeout_ms; + } + +private: + RcConfig cfg_; + std::atomic running_{false}; + std::thread thread_; + int fd_ = -1; + + std::mutex mutex_; + std::map channels_; + std::chrono::steady_clock::time_point last_rx_time_{}; + bool port_open_ = false; + + std::string line_buf_; + + 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); + + struct termios options; + tcgetattr(fd_, &options); + + speed_t speed = B115200; + switch (cfg_.baud) { + case 9600: + speed = B9600; + break; + case 19200: + speed = B19200; + break; + case 38400: + speed = B38400; + break; + case 57600: + speed = B57600; + break; + default: + speed = B115200; + break; + } + + cfsetispeed(&options, speed); + cfsetospeed(&options, speed); + + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_cflag &= ~CRTSCTS; + options.c_cflag |= CREAD | CLOCAL; + + // 텍스트 라인 프로토콜이므로 canonical 모드(라인 단위)로 사용 + options.c_lflag |= ICANON; + options.c_lflag &= ~(ECHO | ECHOE | ISIG); + options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK | + ISTRIP | INLCR | IGNCR | ICRNL); + options.c_oflag &= ~OPOST; + + tcsetattr(fd_, TCSANOW, &options); + tcflush(fd_, TCIOFLUSH); + return true; + } + + void closePort() { + if (fd_ >= 0) { + close(fd_); + fd_ = -1; + } + std::lock_guard lock(mutex_); + port_open_ = false; + } + + void parseLine(const std::string &line) { + std::map parsed; + size_t pos = 0; + while (pos <= line.size()) { + size_t comma = line.find(',', pos); + std::string tok = line.substr( + pos, comma == std::string::npos ? std::string::npos : comma - pos); + + size_t b = tok.find_first_not_of(" \t\r\n"); + size_t e = tok.find_last_not_of(" \t\r\n"); + if (b != std::string::npos) { + tok = tok.substr(b, e - b + 1); + if (tok.size() >= 4 && (tok[0] == 'C' || tok[0] == 'c') && + (tok[1] == 'H' || tok[1] == 'h')) { + size_t colon = tok.find(':'); + if (colon != std::string::npos) { + try { + int ch_num = std::stoi(tok.substr(2, colon - 2)); + int val = std::stoi(tok.substr(colon + 1)); + parsed[ch_num] = val; + } catch (...) { + // 손상된 토큰은 무시 + } + } + } + } + + if (comma == std::string::npos) + break; + pos = comma + 1; + } + + if (!parsed.empty()) { + std::lock_guard 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( + now - last_open_attempt) + .count() > 1000) { + last_open_attempt = now; + if (openPort()) { + std::lock_guard 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) { + line_buf_.append(buf, static_cast(n)); + size_t nl; + 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) { + std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n"; + closePort(); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } + } +}; int main(int argc, char **argv) { std::signal(SIGINT, signalHandler); @@ -618,7 +840,7 @@ int main(int argc, char **argv) { uint8_t id1 = 1; uint8_t id2 = 2; bool bcast_mode = false; - float max_v = 0.40f; // m/s + RcConfig rc_cfg; float max_spin_v = 0.30f; // m/s float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm @@ -738,6 +960,46 @@ int main(int argc, char **argv) { max_z = std::stof(argv[++i]); else if (arg == "--no_lidar") 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_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; @@ -761,10 +1023,16 @@ int main(int argc, char **argv) { std::cout << " - 필터링 범위: 높이 Z(" << min_z << "m ~ " << max_z << "m) | 회피 정밀 전폭 " << (robot_width + 0.10f) * 1000.0f << "mm\n"; - std::cout << " - 최고 속도: " << max_v << "m/s | 가속: " << accel_rate - << "RPM/s | Jerk: " << jerk_rate << "RPM/s² (제자리회전: " - << spin_jerk_rate << "RPM/s²) | 회전 곡선혼합: " - << spin_arc_bias * 100.0f << "%\n"; + std::cout << " - 최고 속도: 저속 " << rc_cfg.v_max_low << "m/s / 고속 " + << rc_cfg.v_max_high << "m/s (CH" << rc_cfg.ch_speed + << " 전환) | 가속: " << accel_rate << "RPM/s | Jerk: " << jerk_rate + << "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 << " | 걸림 속도비 " << stall_vel_ratio << " / 전류 " << stall_current_a << "A 이상\n"; @@ -783,7 +1051,7 @@ int main(int argc, char **argv) { if (!log_path.empty()) { log_stream.open(log_path); 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," "cmd_fl,cmd_fr,cmd_rl,cmd_rr," "fb_fl,fb_fr,fb_rl,fb_rr," "amp_fl,amp_fr,amp_rl,amp_rr," @@ -809,26 +1077,11 @@ int main(int argc, char **argv) { } } - if (SDL_Init(SDL_INIT_JOYSTICK) < 0) { - std::cerr << "[오류] SDL 조이스틱 초기화 실패: " << SDL_GetError() - << std::endl; - return 1; - } - - if (SDL_NumJoysticks() < 1) { - std::cerr << "[경고] 무선 Xbox 컨트롤러(조이스틱)가 연결되지 않았습니다.\n"; - SDL_Quit(); - return 1; - } - - SDL_Joystick *joystick = SDL_JoystickOpen(0); - if (!joystick) { - std::cerr << "[오류] 조이스틱 열기 실패!\n"; - SDL_Quit(); - return 1; - } - std::cout << "[정보] C++ Xbox 컨트롤러 연결 성공: " - << SDL_JoystickName(joystick) << std::endl; + RcReceiver rc(rc_cfg); + rc.start(); + std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 (" + << rc_cfg.port << "). 신호 수신 전까지는 안전을 위해 브레이크가 " + "걸린 상태로 대기합니다.\n"; SerialPort sp1, sp2; if (!sp1.openPort(port1)) @@ -887,8 +1140,9 @@ int main(int argc, char **argv) { } } - std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 " - "또는 Ctrl+C, 라이다 토글: X 버튼)\n\n"; + std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: Ctrl+C, " + "라이다 회피 " << (use_lidar ? "활성화" : "비활성화") + << " 상태로 시작 - 변경하려면 --no_lidar 옵션을 사용하세요)\n\n"; float target_fl = 0.0f, target_fr = 0.0f; float target_rl = 0.0f, target_rr = 0.0f; @@ -950,41 +1204,70 @@ int main(int argc, char **argv) { bool just_landed_rl = false, just_landed_rr = false; char wheel_status_str[5] = "OOOO"; 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 조향 최대 각속도 while (g_running) { - SDL_Event event; - while (SDL_PollEvent(&event)) { - if (event.type == SDL_JOYBUTTONDOWN) { - if (event.jbutton.button == 0) { // A 버튼: 트립 리셋 - trip_initialized = false; - } else if (event.jbutton.button == - 2) { // X 버튼: 라이다 장애물 회피 토글 - use_lidar = !use_lidar; - std::cout << "\n[정보] 라이다 회피 기능: " - << (use_lidar ? "ON" : "OFF") << std::endl; - } else if (event.jbutton.button == 1 || - event.jbutton.button == 6) { // B or Back - g_running = 0; - } + // --------------------------------------------------------------------- + // RC 수신기 채널 읽기 (doc/joystick.md §2, §3 기능 명세서 기준) + // --------------------------------------------------------------------- + int raw_steer = rc_cfg.steer_center_lo, raw_throttle = 1500; + int raw_brake = 1011, raw_speed_mode = 1011; + bool rc_ok = rc.isConnected(); + rc.getChannel(rc_cfg.ch_steer, raw_steer); + rc.getChannel(rc_cfg.ch_throttle, raw_throttle); + rc.getChannel(rc_cfg.ch_brake, raw_brake); + rc.getChannel(rc_cfg.ch_speed, raw_speed_mode); + + if (!rc_ok && prev_rc_ok) { + std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms + << "ms 이상 미수신) -> 비상 정지.\n"; + } + prev_rc_ok = rc_ok; + + // 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리 + bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold); + + // 3.2 속도 제한 설정 (CH7) + 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 + + if (!brake_on) { + // 3.3 전후진 속도 계산 (CH2) + 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(rc_cfg.throttle_center_lo - t) / + static_cast(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) * + v_max_now; + } else { + v_x = -(static_cast(t - rc_cfg.throttle_center_hi) / + static_cast(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(rc_cfg.steer_center_lo - s) / + static_cast(rc_cfg.steer_center_lo - rc_cfg.steer_min)) * + omega_max; + } else { + omega = (static_cast(s - rc_cfg.steer_center_hi) / + static_cast(rc_cfg.steer_max - rc_cfg.steer_center_hi)) * + omega_max; } } - Sint16 lx_raw = SDL_JoystickGetAxis(joystick, 0); - Sint16 ly_raw = SDL_JoystickGetAxis(joystick, 1); - Sint16 rx_raw = SDL_JoystickGetAxis(joystick, 3); - - float ly = applyDeadzone(-static_cast(ly_raw) / 32767.0f); - float lx = applyDeadzone(static_cast(lx_raw) / 32767.0f); - float rx = applyDeadzone(static_cast(rx_raw) / 32767.0f); - - if (std::abs(lx) < 0.15f) - lx = 0.0f; - if (std::abs(rx) < 0.15f) - rx = 0.0f; - - float v_x = ly * max_v; // 선속도 m/s - float omega = 0.0f; // 각속도 rad/s - // --------------------------------------------------------------------- // LiDAR Obstacle Avoidance & Stopping Logic Integration // (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작) @@ -1011,8 +1294,8 @@ int main(int argc, char **argv) { } } - // 2. Side Dodge Steering (전진 중에만 동작) - if (v_x > 0.01f && rx == 0.0f) { + // 2. Side Dodge Steering (전진 중, 사용자 조향 입력이 없을 때만 동작) + if (v_x > 0.01f && omega == 0.0f) { float right_intensity = 0.0f; float left_intensity = 0.0f; @@ -1054,17 +1337,18 @@ int main(int argc, char **argv) { } } - // 제자리 회전 여부: 왼쪽 스틱 전후(ly) 입력이 완전히 중립일 때만 진입한다. - // 그렇지 않으면(주행 중 오른쪽 스틱이 드리프트 등으로 미세하게라도 감지될 - // 경우) 전진 입력이 통째로 무시되고 제자리 회전 로직으로 튀어버려, 커브 - // 선회 중에 갑자기 이상하게 회전하려는 문제가 생긴다. ly는 applyDeadzone을 - // 거쳐 데드존 이하일 때 정확히 0.0f를 반환하므로 등호 비교가 안전하다. - bool is_spin_turn = std::abs(rx) > 0.0f && ly == 0.0f; + // 제자리 회전 여부: CH2(전후진) 입력이 완전히 중립(v_x == 0.0f)이면서 + // CH1(조향) 입력이 있을 때만 진입한다. v_x는 데드존 처리 시 정확히 + // 0.0f로 설정되므로 등호 비교가 안전하다. + bool is_spin_turn = (v_x == 0.0f) && (std::abs(omega) > 0.0f); if (is_spin_turn) { - // 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합 - omega = -rx * (max_spin_v / (effective_w / 2.0f)); - float v_bias = std::abs(rx) * max_spin_v * spin_arc_bias; + // 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합 (스크럽 + // 마찰 저항 감소를 위해 ICR을 로봇 중심에서 살짝 벗어나게 함) + 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_r = v_bias + omega * (effective_w / 2.0f); @@ -1073,33 +1357,13 @@ int main(int argc, char **argv) { target_rl = v_l * rpm_per_ms; target_rr = -v_r * rpm_per_ms; } else { - // 직진 및 차동 선회 Kinematics - 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_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; - } 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; - } + // 직진 및 차동 선회 Kinematics (CH1 조향 + 라이다 회피 오프셋 포함) + 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으로 낮춰 @@ -1114,7 +1378,26 @@ int main(int argc, char **argv) { if (airborne_rr) target_rr = 0.0f; - if (state == "STOPPED") { + // 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시 + // 0으로 스냅 후 브레이크를 잠근다. 해제되면 STOPPED 상태에서 아래 + // 상태머신이 정상적으로 재가속을 시작한다. + 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"; + } + 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) { driver_front.setBrakes(false); if (driver_rear_ptr) @@ -1333,8 +1616,10 @@ int main(int argc, char **argv) { float t_ms = std::chrono::duration( std::chrono::steady_clock::now() - log_start_time) .count(); - log_stream << t_ms << ',' << state << ',' << ly << ',' << lx << ',' - << rx << ',' << cmd_fl << ',' << cmd_fr << ',' << cmd_rl + log_stream << t_ms << ',' << state << ',' << raw_steer << ',' + << raw_throttle << ',' << (rc_ok ? 1 : 0) << ',' + << (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ',' + << omega << ',' << cmd_fl << ',' << cmd_fr << ',' << cmd_rl << ',' << cmd_rr << ',' << fl_fb << ',' << fr_fb << ',' << rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp << ',' << rl_amp << ',' << rr_amp << ',' << wheel_status_str[0] @@ -1344,9 +1629,10 @@ int main(int argc, char **argv) { << dist_axle << ',' << comm_ms << '\n'; } - printf("\r\033[K[%-8s] AXLE:%5.3fm | %-28s | I(A):FL%+4.1f FR%+4.1f " + printf("\r\033[K[%-8s] RC:%s%s 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(), dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp, + state.c_str(), rc_ok ? "OK" : "LOST", brake_on ? "[BRK]" : " ", + dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp, rl_amp, rr_amp, wheel_status_str, fault_active ? " [!!알람!!]" : "", comm_ms); fflush(stdout); @@ -1373,9 +1659,7 @@ int main(int argc, char **argv) { std::cout << "[정보] CSV 로그 저장 완료: " << log_path << "\n"; } - if (joystick) - SDL_JoystickClose(joystick); - SDL_Quit(); + rc.stop(); std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n"; return 0; diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp index 417afb1..59e3167 100755 Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ