diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..333c1e9 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +logs/ diff --git a/imu/imu_test b/imu/imu_test new file mode 100644 index 0000000..bed989d --- /dev/null +++ b/imu/imu_test @@ -0,0 +1,236 @@ +// HWT905-RS232 (WitMotion) IMU reader — Phase 1: pure observation, no control loop. +// Reads the sensor's active-push protocol (0x55-prefixed packets) over a plain +// serial port (RS232-over-USB, e.g. /dev/ttyUSB0) and prints accel/gyro/angle to +// stdout, optionally logging to CSV. Not Modbus — the RS485 variant uses Modbus, +// this RS232 variant does not. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +constexpr uint8_t kFrameHeader = 0x55; +constexpr uint8_t kTypeAccel = 0x51; +constexpr uint8_t kTypeGyro = 0x52; +constexpr uint8_t kTypeAngle = 0x53; +constexpr uint8_t kTypeMag = 0x54; + +struct ImuState { + double accel[3] = {0, 0, 0}; // g + double gyro[3] = {0, 0, 0}; // deg/s + double angle[3] = {0, 0, 0}; // deg (roll, pitch, yaw) + double mag[3] = {0, 0, 0}; // raw counts + bool has_accel = false, has_gyro = false, has_angle = false, has_mag = false; +}; + +int16_t toInt16(uint8_t lo, uint8_t hi) { + return static_cast(static_cast(lo) | (static_cast(hi) << 8)); +} + +speed_t baudToSpeed(int baud) { + switch (baud) { + case 4800: return B4800; + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + default: + std::cerr << "[경고] 지원하지 않는 baud " << baud << ", 115200으로 대체\n"; + return B115200; + } +} + +int openSerialPort(const std::string &port, int baud) { + int fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "[오류] 포트 열기 실패: " << port << " (" << std::strerror(errno) << ")\n"; + return -1; + } + fcntl(fd, F_SETFL, 0); // switch back to blocking reads + + struct termios options; + if (tcgetattr(fd, &options) != 0) { + std::cerr << "[오류] tcgetattr 실패\n"; + close(fd); + return -1; + } + + speed_t speed = baudToSpeed(baud); + cfsetispeed(&options, speed); + cfsetospeed(&options, speed); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_cflag &= ~CRTSCTS; + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_iflag &= ~(IXON | IXOFF | IXANY); + options.c_iflag &= ~(INLCR | ICRNL); + options.c_oflag &= ~OPOST; + + options.c_cc[VMIN] = 1; + options.c_cc[VTIME] = 5; // 0.5s inter-byte timeout + + tcflush(fd, TCIFLUSH); + if (tcsetattr(fd, TCSANOW, &options) != 0) { + std::cerr << "[오류] tcsetattr 실패\n"; + close(fd); + return -1; + } + return fd; +} + +// Parses one validated 11-byte WitMotion frame (frame[0]==0x55, checksum ok) +// into the running ImuState. Returns true if this frame completed an +// accel+gyro+angle group worth printing (i.e. it was an angle frame). +bool applyFrame(const uint8_t *frame, ImuState &state) { + const uint8_t type = frame[1]; + switch (type) { + case kTypeAccel: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.accel[i] = raw / 32768.0 * 16.0; // g + } + state.has_accel = true; + return false; + case kTypeGyro: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.gyro[i] = raw / 32768.0 * 2000.0; // deg/s + } + state.has_gyro = true; + return false; + case kTypeAngle: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.angle[i] = raw / 32768.0 * 180.0; // deg + } + state.has_angle = true; + return true; + case kTypeMag: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + state.mag[i] = raw; + } + state.has_mag = true; + return false; + default: + return false; // time/quaternion/GPS frames etc. — ignored in Phase 1 + } +} + +} // namespace + +int main(int argc, char **argv) { + std::setvbuf(stdout, nullptr, _IOLBF, 4096); // line-buffer even when piped + + std::string port = "/dev/ttyUSB0"; + int baud = 9600; // HWT905-232 factory default (confirmed against this unit) + std::string log_path; + + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--port" && i + 1 < argc) { + port = argv[++i]; + } else if (arg == "--baud" && i + 1 < argc) { + baud = std::stoi(argv[++i]); + } else if (arg == "--log" && i + 1 < argc) { + log_path = argv[++i]; + } else if (arg == "--help") { + std::cout << "사용법: " << argv[0] + << " [--port /dev/ttyUSB0] [--baud 115200] [--log out.csv]\n"; + return 0; + } + } + + int fd = openSerialPort(port, baud); + if (fd < 0) return 1; + + std::ofstream log_file; + if (!log_path.empty()) { + log_file.open(log_path); + if (!log_file) { + std::cerr << "[오류] 로그 파일 열기 실패: " << log_path << "\n"; + return 1; + } + log_file << "t_s,ax_g,ay_g,az_g,gx_dps,gy_dps,gz_dps,roll_deg,pitch_deg,yaw_deg\n"; + } + + std::cout << "포트 " << port << " @ " << baud << "bps 에서 IMU 데이터 수신 시작 " + << "(Ctrl+C로 종료)\n"; + std::cout.flush(); + + const auto t_start = std::chrono::steady_clock::now(); + ImuState state; + uint8_t buf[11]; + size_t buf_len = 0; + + while (true) { + uint8_t byte; + ssize_t n = read(fd, &byte, 1); + if (n <= 0) { + if (n < 0 && errno != EAGAIN && errno != EINTR) { + std::cerr << "[오류] 시리얼 읽기 실패: " << std::strerror(errno) << "\n"; + break; + } + continue; + } + + if (buf_len == 0) { + if (byte != kFrameHeader) continue; // resync: wait for header + buf[buf_len++] = byte; + continue; + } + + buf[buf_len++] = byte; + if (buf_len < 11) continue; + + // Full 11-byte candidate frame collected — validate checksum. + uint8_t sum = 0; + for (int i = 0; i < 10; ++i) sum += buf[i]; + if (sum != buf[10]) { + // Checksum mismatch: resync by sliding one byte and rescanning for 0x55. + std::cerr << "[경고] 체크섬 불일치, 프레임 폐기\n"; + buf_len = 0; + continue; + } + + bool print_now = applyFrame(buf, state); + buf_len = 0; + + if (print_now && state.has_accel && state.has_gyro && state.has_angle) { + double t_s = std::chrono::duration(std::chrono::steady_clock::now() - t_start).count(); + std::printf( + "t=%7.3fs accel[g]=(%+.3f,%+.3f,%+.3f) gyro[dps]=(%+7.2f,%+7.2f,%+7.2f) " + "angle[deg]=(roll=%+7.2f,pitch=%+7.2f,yaw=%+7.2f)\n", + t_s, state.accel[0], state.accel[1], state.accel[2], state.gyro[0], state.gyro[1], + state.gyro[2], state.angle[0], state.angle[1], state.angle[2]); + + if (log_file) { + log_file << t_s << ',' << state.accel[0] << ',' << state.accel[1] << ',' + << state.accel[2] << ',' << state.gyro[0] << ',' << state.gyro[1] << ',' + << state.gyro[2] << ',' << state.angle[0] << ',' << state.angle[1] << ',' + << state.angle[2] << '\n'; + log_file.flush(); + } + } + } + + close(fd); + return 0; +} diff --git a/run_4wd.sh b/run_4wd.sh index a00b7bb..adadd53 100755 --- a/run_4wd.sh +++ b/run_4wd.sh @@ -1,9 +1,11 @@ #!/usr/bin/env bash # 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance -# 현재 고정 배선: 모터 드라이버 = ttyUSB1, RC 수신기(XR1) = ttyUSB0 -PORT="${1:-/dev/ttyUSB1}" -RC_PORT="${RC_PORT:-/dev/ttyUSB0}" +# udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된 심볼릭 +# 링크 사용 — ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름들은 고정이다. +PORT="${1:-/dev/ttyMOTOR}" +RC_PORT="${RC_PORT:-/dev/ttyRC}" +IMU_PORT="${IMU_PORT:-/dev/ttyIMU}" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" cd "$SCRIPT_DIR" || exit 1 @@ -12,11 +14,12 @@ echo "==================================================================" echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템" echo "==================================================================" -# 1. USB Latency 1ms 단축 (모터 포트 + RC 수신기 포트 둘 다) +# 1. USB Latency 1ms 단축 (모터/RC/IMU 포트 전부) if [ -f "./set_low_latency.sh" ]; then - echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT)" + echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT / IMU: $IMU_PORT)" ./set_low_latency.sh "$PORT" ./set_low_latency.sh "$RC_PORT" + ./set_low_latency.sh "$IMU_PORT" fi # 2. C++ 바이너리 존재 여부 확인 및 컴파일 @@ -26,16 +29,17 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then fi echo "==================================================================" -echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + 라이다 장애물 회피 시작" -echo " - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT" -echo " - 라이다 기능 비활성화: --no_lidar 옵션" +echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + IMU(HWT905) + 라이다 장애물 회피 시작" +echo " - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT / IMU 포트: $IMU_PORT" +echo " - 라이다 기능 비활성화: --no_lidar 옵션 / IMU 비활성화: --no_imu 옵션" +echo " - 매 주행마다 logs/에 CSV 로그 자동 저장 (끄려면 --no_log)" echo " - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C" echo "==================================================================" -# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port를 다시 넘기면 -# 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨) +# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port/--imu_port를 다시 +# 넘기면 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨) if [ $# -gt 1 ]; then - ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" "${@:2}" + ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT" "${@:2}" else - ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" + ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT" fi diff --git a/set_low_latency.sh b/set_low_latency.sh index d917b56..cea656d 100755 --- a/set_low_latency.sh +++ b/set_low_latency.sh @@ -8,7 +8,10 @@ if [ ! -e "$PORT" ]; then exit 1 fi -DEV_NAME=$(basename "$PORT") +# /dev/ttyMOTOR 같은 udev 고정 심볼릭 링크로 넘어올 수 있으므로, sysfs 조회에 +# 필요한 실제 장치명(ttyUSB0 등)으로 반드시 풀어준다 — 안 그러면 +# /sys/bus/usb-serial/devices/ttyMOTOR 경로가 존재하지 않아 항상 폴백으로 샌다. +DEV_NAME=$(basename "$(readlink -f "$PORT")") LATENCY_PATH="/sys/bus/usb-serial/devices/$DEV_NAME/latency_timer" if [ -f "$LATENCY_PATH" ]; then diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp index 4af3b35..34b7a01 100644 --- a/xbox_motor_control.cpp +++ b/xbox_motor_control.cpp @@ -5,7 +5,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -315,33 +317,39 @@ public: bool readFeedback(float &l_fb, float &r_fb, int32_t &l_tick, int32_t &r_tick, float &l_torque_a, float &r_torque_a, - uint16_t &err_l, uint16_t &err_r) { + uint16_t &err_l, uint16_t &err_r, int &l_temp_c, + int &r_temp_c) { std::vector regs; - // 0x20A5~0x20AE 10레지스터 연속 읽기: 에러코드(0x20A5/0x20A6)부터 - // 포지션 틱, RPM 피드백, 실제 토크(전류)까지 한 번의 통신으로 확보. + // 0x20A4~0x20AE 11레지스터 연속 읽기: 모터온도(0x20A4)부터 에러코드 + // (0x20A5/0x20A6), 포지션 틱, RPM 피드백, 실제 토크(전류)까지 한 번의 + // 통신으로 확보. 0x20A4는 기존 읽기 범위 바로 앞이라 추가 통신 없이 + // 온도까지 같이 받아온다(과열 사전감지용, doc/02 §읽기전용상태 참고). // 무부하(공중에 뜬) 바퀴는 전류가 급격히 낮아지므로 슬립 진단에 사용, // 에러코드는 과전류/과부하 등 드라이버 알람 발생 시 원인 진단에 사용. - if (port_->readRegs(slave_id_, 0x20A5, 10, regs)) { - err_l = regs[0]; - err_r = regs[1]; + if (port_->readRegs(slave_id_, 0x20A4, 11, regs)) { + l_temp_c = static_cast((regs[0] >> 8) & 0xFF); + r_temp_c = static_cast(regs[0] & 0xFF); + + err_l = regs[1]; + err_r = regs[2]; // 수정: uint32_t -> int32_t 캐스팅은 2의 보수 표현에서 안전하게 부호가 // 재해석됨. 기존의 "val - 0x100000000ULL" 방식은 uint64_t 승격 후 // int32_t로 축소 캐스팅하는 구현정의 동작(현실적으로는 대부분 동작하지만 // 명확성/이식성이 떨어짐)이라 제거함. - uint32_t val_l = (static_cast(regs[2]) << 16) | regs[3]; + uint32_t val_l = (static_cast(regs[3]) << 16) | regs[4]; l_tick = static_cast(val_l); - uint32_t val_r = (static_cast(regs[4]) << 16) | regs[5]; + uint32_t val_r = (static_cast(regs[5]) << 16) | regs[6]; r_tick = static_cast(val_r); - int16_t vl = static_cast(regs[6]); - int16_t vr = static_cast(regs[7]); + int16_t vl = static_cast(regs[7]); + int16_t vr = static_cast(regs[8]); l_fb = static_cast(vl) * 0.1f; // 0.1RPM 단위 -> RPM r_fb = static_cast(vr) * 0.1f; - int16_t tl = static_cast(regs[8]); - int16_t tr = static_cast(regs[9]); + int16_t tl = static_cast(regs[9]); + int16_t tr = static_cast(regs[10]); l_torque_a = static_cast(tl) * 0.1f; // 0.1A 단위 -> A r_torque_a = static_cast(tr) * 0.1f; return true; @@ -349,6 +357,17 @@ public: return false; } + // 드라이버 자체 온도(0x20B0, 0.1℃). 열 시정수가 초 단위로 느려서 매 틱 + // 읽을 필요가 없어 별도 저빈도 폴링용으로 분리(readFeedback 연속범위와 + // 떨어져 있어 합치면 통신 1회가 늘어남). + bool readDriverTemp(float &temp_c) { + std::vector regs; + if (!port_->readRegs(slave_id_, 0x20B0, 1, regs)) + return false; + temp_c = static_cast(regs[0]) * 0.1f; + return true; + } + // 정격전류(0x2033/0x2063)·최대전류(0x2034/0x2064) 실측 조회. 채널당 // 2레지스터씩 떨어져 있어 L/R 두 번 읽음(설정값이라 시작 시 1회만 조회). bool readCurrentLimits(uint16_t &rated_l, uint16_t &max_l, uint16_t &rated_r, @@ -628,7 +647,9 @@ struct termios2 { // 의 커널 ABI와 동일 레이아웃 (glib }; struct RcConfig { - std::string port = "/dev/ttyUSB0"; // 현재 고정 배선: RC 수신기(XR1) = ttyUSB0 + // udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된 + // 심볼릭 링크. ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름은 고정. + std::string port = "/dev/ttyRC"; int baud = 420000; // CRSF 표준 보드레이트 int ch_steer = 1; // CH1: 좌우 조향 @@ -914,20 +935,285 @@ private: } }; +// ----------------------------------------------------------------------------- +// WitMotion HWT905-RS232 IMU 리더 (doc/06-imu-integration-plan.md Phase 1) +// RS485/Modbus 변형과 달리 이 RS232 변형은 액티브 푸시 방식 바이너리 프로토콜을 +// 쓴다: [0]=0x55 헤더, [1]=타입(0x51 가속도/0x52 자이로/0x53 각도/0x54 지자기), +// [2..9]=int16×4(LE, XYZ+예약), [10]=체크섬(0~9바이트 합의 하위 8비트). +// imu/imu_test에서 실기로 검증된 프로토콜/스케일을 그대로 이식했다. +// Phase 1 범위: 순수 관측만 한다 — 제어 루프에는 절대 개입하지 않는다. +// ----------------------------------------------------------------------------- +struct ImuConfig { + // udev 규칙으로 고정된 심볼릭 링크 (모터=/dev/ttyMOTOR, RC=/dev/ttyRC와 별도) + std::string port = "/dev/ttyIMU"; + int baud = 9600; // HWT905-232 공장 출하 기본값(실기 확인됨) + int failsafe_timeout_ms = 500; // 이 시간 이상 미수신 시 미연결로 표시 +}; + +class ImuReader { +public: + struct Snapshot { + double accel[3] = {0, 0, 0}; // g (x,y,z) + double gyro[3] = {0, 0, 0}; // deg/s (x,y,z) + double angle[3] = {0, 0, 0}; // deg (roll, pitch, yaw) + bool valid = false; + }; + + explicit ImuReader(const ImuConfig &cfg) : cfg_(cfg) {} + ~ImuReader() { stop(); } + + void start() { + running_ = true; + thread_ = std::thread(&ImuReader::run, this); + } + + void stop() { + running_ = false; + if (thread_.joinable()) + thread_.join(); + closePort(); + } + + bool isConnected() { + std::lock_guard lock(mutex_); + if (!port_open_ || !has_angle_) + return false; + auto now = std::chrono::steady_clock::now(); + return std::chrono::duration_cast( + now - last_rx_time_) + .count() <= cfg_.failsafe_timeout_ms; + } + + Snapshot getSnapshot() { + std::lock_guard lock(mutex_); + Snapshot s; + for (int i = 0; i < 3; ++i) { + s.accel[i] = accel_[i]; + s.gyro[i] = gyro_[i]; + s.angle[i] = angle_[i]; + } + s.valid = isConnectedLocked(); + return s; + } + +private: + ImuConfig cfg_; + std::atomic running_{false}; + std::thread thread_; + int fd_ = -1; + + std::mutex mutex_; + double accel_[3] = {0, 0, 0}; + double gyro_[3] = {0, 0, 0}; + double angle_[3] = {0, 0, 0}; + bool has_accel_ = false, has_gyro_ = false, has_angle_ = false; + std::chrono::steady_clock::time_point last_rx_time_{}; + bool port_open_ = false; + + static constexpr uint8_t kFrameHeader = 0x55; + static constexpr uint8_t kTypeAccel = 0x51; + static constexpr uint8_t kTypeGyro = 0x52; + static constexpr uint8_t kTypeAngle = 0x53; + + bool isConnectedLocked() const { + if (!port_open_ || !has_angle_) + return false; + auto now = std::chrono::steady_clock::now(); + return std::chrono::duration_cast( + now - last_rx_time_) + .count() <= cfg_.failsafe_timeout_ms; + } + + static int16_t toInt16(uint8_t lo, uint8_t hi) { + return static_cast(static_cast(lo) | + (static_cast(hi) << 8)); + } + + static speed_t baudToSpeed(int baud) { + switch (baud) { + case 4800: + return B4800; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + default: + return B9600; + } + } + + bool openPort() { + fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if (fd_ < 0) + return false; + fcntl(fd_, F_SETFL, 0); // 블로킹 읽기로 전환 + + struct termios options; + if (tcgetattr(fd_, &options) != 0) { + close(fd_); + fd_ = -1; + return false; + } + + speed_t speed = baudToSpeed(cfg_.baud); + cfsetispeed(&options, speed); + cfsetospeed(&options, speed); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_cflag &= ~CRTSCTS; + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_iflag &= ~(IXON | IXOFF | IXANY); + options.c_iflag &= ~(INLCR | ICRNL); + options.c_oflag &= ~OPOST; + + options.c_cc[VMIN] = 1; + options.c_cc[VTIME] = 5; // 0.5초 바이트 간 타임아웃 (종료 감지용) + + tcflush(fd_, TCIFLUSH); + if (tcsetattr(fd_, TCSANOW, &options) != 0) { + close(fd_); + fd_ = -1; + return false; + } + return true; + } + + void closePort() { + if (fd_ >= 0) { + close(fd_); + fd_ = -1; + } + std::lock_guard lock(mutex_); + port_open_ = false; + } + + // 검증된 11바이트 프레임 하나를 상태에 반영. 각도 프레임 완료 시 true. + bool applyFrame(const uint8_t *frame) { + const uint8_t type = frame[1]; + switch (type) { + case kTypeAccel: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + accel_[i] = raw / 32768.0 * 16.0; // g + } + has_accel_ = true; + return false; + case kTypeGyro: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + gyro_[i] = raw / 32768.0 * 2000.0; // deg/s + } + has_gyro_ = true; + return false; + case kTypeAngle: + for (int i = 0; i < 3; ++i) { + int16_t raw = toInt16(frame[2 + 2 * i], frame[3 + 2 * i]); + angle_[i] = raw / 32768.0 * 180.0; // deg + } + has_angle_ = true; + return true; + default: + return false; // 시간/쿼터니언/GPS 등: Phase 1에서는 무시 + } + } + + void run() { + auto last_open_attempt = + std::chrono::steady_clock::now() - std::chrono::milliseconds(1001); + uint8_t buf[11]; + size_t buf_len = 0; + + 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[정보] IMU 포트(" << cfg_.port << ") 연결 성공.\n"; + } else { + std::cerr << "\n[경고] IMU 포트(" << cfg_.port + << ") 열기 실패. 1초 후 재시도.\n"; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + buf_len = 0; + continue; + } + + uint8_t byte; + ssize_t n = read(fd_, &byte, 1); + if (n <= 0) { + if (n < 0 && errno != EAGAIN && errno != EINTR) { + std::cerr << "\n[경고] IMU 포트 통신 오류. 재연결 시도...\n"; + closePort(); + buf_len = 0; + } + continue; // n==0: VTIME 타임아웃, running_ 재확인 후 계속 대기 + } + + if (buf_len == 0) { + if (byte != kFrameHeader) + continue; // 헤더 대기하며 재동기화 + buf[buf_len++] = byte; + continue; + } + + buf[buf_len++] = byte; + if (buf_len < 11) + continue; + + uint8_t sum = 0; + for (int i = 0; i < 10; ++i) + sum += buf[i]; + if (sum != buf[10]) { + buf_len = 0; // 체크섬 불일치: 프레임 폐기 후 재동기화 + continue; + } + + bool angle_done; + { + std::lock_guard lock(mutex_); + angle_done = applyFrame(buf); + if (angle_done) + last_rx_time_ = std::chrono::steady_clock::now(); + } + buf_len = 0; + (void)angle_done; + } + } +}; + int main(int argc, char **argv) { std::signal(SIGINT, signalHandler); std::signal(SIGTERM, signalHandler); - std::string port1 = "/dev/ttyUSB1"; // 현재 고정 배선: 모터 드라이버 = ttyUSB1 + // udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된 + // 심볼릭 링크. ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름은 고정. + std::string port1 = "/dev/ttyMOTOR"; std::string port2 = ""; uint8_t id1 = 1; uint8_t id2 = 2; bool bcast_mode = false; RcConfig rc_cfg; float max_spin_v = 0.30f; // m/s - float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm - float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름 + float k_skid_override = -1.0f; // 0 이상이면 Phase 5a 기본 캘리브레이션값을 대체 + float effective_w_override = -1.0f; // 0 이상이면 Phase 5a 기본 캘리브레이션값을 대체 // 가속은 역기전력 보호회로가 하드웨어로 구성되어 목표값에 1:1로 즉시 // 추종하므로 별도의 가속 한계값이 없다. decel_rate/jerk_rate(및 // spin_jerk_rate)는 감속에만 적용된다. @@ -939,6 +1225,27 @@ int main(int argc, char **argv) { // 살짝 벗어나게 해 스크럽 마찰 저항을 줄임) float bumper_offset = 0.045f; // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리 + // Phase 3 — 직진 헤딩 홀드 PI 트림 (doc/06-imu-integration-plan.md §Phase 3). + // 조향 중립 + 라이다 회피 미개입 + 실제 주행 중일 때만 개입해, 진입 순간의 + // fused_yaw_deg를 목표로 고정하고 그로부터 벗어난 만큼을 PI로 보정한다. + bool heading_hold_enabled = true; + float heading_kp = 0.6f; // rad/s per rad 오차 + float heading_ki = 0.15f; // rad/s per (rad·s) 적분 오차 + float heading_max_trim = 0.15f; // rad/s (트림 상한 — 의도적 조향을 압도하지 않도록) + + // Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (doc §Phase 4). 지령 + // omega 대비 바퀴 피드백과 무관한 IMU 원시 gyro_z 비율이 임계값 밑으로 + // 지속되면 "전신 슬립"(빙판/젖은 잔디 등 4륜 동시 헛돎)으로 판정해 라이다 + // 회피의 speed_scale과 같은 패턴으로 속도를 일시적으로 낮춘다. 제자리 + // 회전은 반력 토크로 인한 정상적인 스크럽 손실(지령 대비 40~60% 미달도 + // 흔함)이 있어 문턱값을 낮게(=크게 미달일 때만) 잡아 정상 회전을 오탐하지 + // 않게 한다. + bool whole_slip_enabled = true; + float whole_slip_min_omega = 0.15f; // rad/s (이 미만 지령은 판정 보류) + float whole_slip_ratio_threshold = 0.15f; // |imu_omega|/|omega| 가 이 밑이면 슬립 후보 + int whole_slip_debounce_ticks = 15; // 틱 (100Hz 기준 150ms) 연속돼야 확정 + float whole_slip_speed_scale = 0.5f; // 슬립 확정 시 v_x/omega에 곱하는 배율 + // 바퀴별 이상(들뜸/걸림) 판정 파라미터: 속도 폐루프 특성상 "지령 대비 실제 // 속도" 하나만으로는 무부하(들뜸) 판정이 안 되므로, 동료 바퀴 대비 전류 // 편차와 함께 두 축으로 판정한다. @@ -951,13 +1258,21 @@ int main(int argc, char **argv) { // 지연이 동반되면 걸림/과부하로 판정 int airborne_debounce_ticks = 5; // 틱 (100Hz 기준 50ms) 연속 들뜸 판정 // 시에만 실제로 목표속도를 낮춤 - std::string log_path = ""; // 비어있으면 로깅 비활성화. --log <파일경로>로 지정 + // 비어있으면 프로그램 시작 시 logs/drive_YYYYmmdd_HHMMSS.csv로 자동 생성된다 + // (매 주행마다 RC/IMU/모터 로깅을 자동으로 별도 파일에 남기기 위함). + // --log <파일경로>로 직접 지정하거나 --no_log로 완전히 끌 수 있다. + std::string log_path = ""; + bool no_log = false; // 실측 결과 정격15A/최대30A(공장 기본값) 그대로였음. 정격 위로 5A 여유는 // 남기되, 걸림/과부하 상황에서 30A까지 밀어붙이며 3초씩 버티다 과부하 // 알람이 터지는 걸 막기 위해 기본값을 20A로 낮춤. --max_current_a로 조정 가능. // 음수를 주면 미변경(공장/기존 설정 유지). float max_current_a = 20.0f; + // IMU 파라미터 (doc/06-imu-integration-plan.md Phase 1: 순수 관측) + bool use_imu = true; + ImuConfig imu_cfg; + // LiDAR Parameters & Robot Physical Specs (User Specification) bool use_lidar = true; std::string lidar_config = "mid360s_config.json"; @@ -987,10 +1302,10 @@ int main(int argc, char **argv) { id1 = static_cast(std::stoi(argv[++i])); else if (arg == "--id2" && i + 1 < argc) id2 = static_cast(std::stoi(argv[++i])); - else if (arg == "--track_width" && i + 1 < argc) - track_width = std::stof(argv[++i]); - else if (arg == "--wheelbase" && i + 1 < argc) - wheelbase = std::stof(argv[++i]); + else if (arg == "--k_skid" && i + 1 < argc) + k_skid_override = std::stof(argv[++i]); + else if (arg == "--effective_w" && i + 1 < argc) + effective_w_override = std::stof(argv[++i]); else if (arg == "--radius" && i + 1 < argc) wheel_radius = std::stof(argv[++i]); else if (arg == "--bumper" && i + 1 < argc) @@ -1015,6 +1330,8 @@ int main(int argc, char **argv) { airborne_debounce_ticks = std::stoi(argv[++i]); else if (arg == "--log" && i + 1 < argc) log_path = argv[++i]; + else if (arg == "--no_log") + no_log = true; else if (arg == "--max_current_a" && i + 1 < argc) max_current_a = std::stof(argv[++i]); else if (arg == "--bcast" || arg == "--broadcast") @@ -1041,6 +1358,12 @@ int main(int argc, char **argv) { max_z = std::stof(argv[++i]); else if (arg == "--no_lidar") use_lidar = false; + else if (arg == "--imu_port" && i + 1 < argc) + imu_cfg.port = argv[++i]; + else if (arg == "--imu_baud" && i + 1 < argc) + imu_cfg.baud = std::stoi(argv[++i]); + else if (arg == "--no_imu") + use_imu = false; else if (arg == "--rc_port" && i + 1 < argc) rc_cfg.port = argv[++i]; else if (arg == "--rc_baud" && i + 1 < argc) @@ -1085,15 +1408,55 @@ int main(int argc, char **argv) { 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]); + else if (arg == "--no_heading_hold") + heading_hold_enabled = false; + else if (arg == "--heading_kp" && i + 1 < argc) + heading_kp = std::stof(argv[++i]); + else if (arg == "--heading_ki" && i + 1 < argc) + heading_ki = std::stof(argv[++i]); + else if (arg == "--heading_max_trim" && i + 1 < argc) + heading_max_trim = std::stof(argv[++i]); + else if (arg == "--no_whole_slip") + whole_slip_enabled = false; + else if (arg == "--whole_slip_min_omega" && i + 1 < argc) + whole_slip_min_omega = std::stof(argv[++i]); + else if (arg == "--whole_slip_ratio" && i + 1 < argc) + whole_slip_ratio_threshold = std::stof(argv[++i]); + else if (arg == "--whole_slip_debounce_ticks" && i + 1 < argc) + whole_slip_debounce_ticks = std::stoi(argv[++i]); + else if (arg == "--whole_slip_speed_scale" && i + 1 < argc) + whole_slip_speed_scale = std::stof(argv[++i]); + } + + // --log를 직접 지정하지 않았으면 이 저장소(프로젝트 폴더) 안의 logs/에 + // 타임스탬프 파일명으로 자동 생성한다 — 매 주행마다 RC/IMU/모터 로깅을 + // 빠짐없이 남기기 위함. --no_log로 완전히 끌 수 있다. + if (log_path.empty() && !no_log) { + std::filesystem::create_directories("logs"); + std::time_t now_c = std::time(nullptr); + char ts_buf[32]; + std::strftime(ts_buf, sizeof(ts_buf), "%Y%m%d_%H%M%S", std::localtime(&now_c)); + log_path = std::string("logs/drive_") + ts_buf + ".csv"; } constexpr float PI_VAL = 3.14159265358979323846f; float rpm_per_ms = 60.0f / (2.0f * PI_VAL * wheel_radius); - // 4WD 유효 선회 계수 - float effective_w = - std::sqrt(track_width * track_width + wheelbase * wheelbase); - float k_skid = (track_width + (wheelbase * wheelbase) / track_width) / 2.0f; + // effective_w — Phase 5a 정적 캘리브레이션. 기하학적 공식값(대각선 길이, + // 0.684)은 제자리 회전 실측 대비 작았다. 전용 회전 로그 2개(무결점 + // 405틱, 두 로그 개별 중앙값 0.855/0.904로 서로 일치)에서 역산한 + // 중앙값 0.87을 대신 사용한다. --effective_w로 재조정 가능. + float effective_w = (effective_w_override >= 0.0f) ? effective_w_override : 0.87f; + // k_skid — Phase 5a 정적 캘리브레이션(doc/06-imu-integration-plan.md §5a): + // 기하학적 공식값(트랙폭/휠베이스로만 계산, 0.406)은 실측 대비 계속 + // 작게 나왔다(회전이 지령보다 항상 덜 도는 원인 중 하나). 두 차례 + // 실주행 로그(커브 선회 중 무결점 총 5,150틱)에서 바퀴 실측 속도차 대 + // IMU 실측 회전율로 역산한 중앙값이 0.51 적용 전 0.510, 적용 후에도 + // 0.517로 수렴해 그대로 0.51을 유지한다. 회전 반경에 따라 + // 0.44(완만한 커브)~0.53(급한 커브)로 흔들리는 편이라 0.51은 그 + // 절충값 — 반경별로 정밀하게 맞추려면 Phase 5b(온라인 ICR 추정)가 + // 필요하다. --k_skid로 재조정 가능. + float k_skid = (k_skid_override >= 0.0f) ? k_skid_override : 0.51f; std::cout << "\n=============================================================" "=====\n"; @@ -1129,20 +1492,27 @@ int main(int argc, char **argv) { std::cout << "==================================================================\n"; - // CSV 로깅: --log <경로> 지정 시 매 틱 지령/피드백/전류/휠상태를 파일로 기록. - // 실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로, 문제가 된 - // 구간(턱 넘는 지점 등)을 나중에 잘라서 분석하기 위한 용도. + // CSV 로깅: 기본적으로 매 주행마다 logs/에 자동 저장된다(위 auto-path 로직). + // RC 채널, IMU(자이로/각도), 모터 지령/피드백/전류/휠상태/엔코더 raw tick까지 + // 매 틱 전부 기록한다. 실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로, + // 문제가 된 구간(턱 넘는 지점 등)이나 IMU-엔코더 융합용 데이터를 나중에 잘라서 + // 분석하기 위한 용도. std::ofstream log_stream; if (!log_path.empty()) { log_stream.open(log_path); if (log_stream.is_open()) { log_stream << "t_ms,state,rc_steer,rc_throttle,rc_ok,brake,vmax,vx,omega," "ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8," + "imu_ok,imu_gyro_z,imu_roll,imu_pitch,imu_yaw,imu_yaw_rel," "cmd_fl,cmd_fr,cmd_rl,cmd_rr," "fb_fl,fb_fr,fb_rl,fb_rr," "amp_fl,amp_fr,amp_rl,amp_rr," + "temp_fl,temp_fr,temp_rl,temp_rr,temp_drv_front,temp_drv_rear," "stat_fl,stat_fr,stat_rl,stat_rr," "err_fl,err_fr,err_rl,err_rr," + "tick_fl,tick_fr,tick_rl,tick_rr," + "wheel_omega,imu_omega,omega_residual,fused_yaw,odom_x,odom_y," + "heading_hold,heading_target,heading_trim,whole_slip," "dist_axle,comm_ms\n"; std::cout << " - CSV 로그 기록: " << log_path << "\n"; } else { @@ -1174,6 +1544,11 @@ int main(int argc, char **argv) { "동시에 읽으면 RC 데이터 파싱이 깨질 수 있으니 서로 다른 " "포트로 지정하세요.\n\n"; } + if (use_imu && (imu_cfg.port == port1 || imu_cfg.port == rc_cfg.port)) { + std::cout << "\n[경고] --imu_port(" << imu_cfg.port + << ")가 모터 또는 RC 수신기 포트와 동일합니다. 서로 다른 " + "포트로 지정하세요.\n\n"; + } RcReceiver rc(rc_cfg); rc.start(); @@ -1181,6 +1556,14 @@ int main(int argc, char **argv) { << rc_cfg.port << "). 신호 수신 전까지는 안전을 위해 브레이크가 " "걸린 상태로 대기합니다.\n"; + ImuReader imu(imu_cfg); + if (use_imu) { + imu.start(); + std::cout << "[정보] IMU(HWT905-RS232) 리더 시작 (" << imu_cfg.port << " @" + << imu_cfg.baud << "bps). Phase 1: 관측 전용, 제어에는 개입하지 " + "않습니다.\n"; + } + SerialPort sp1, sp2; if (!sp1.openPort(port1)) return 1; @@ -1268,7 +1651,16 @@ int main(int argc, char **argv) { return target_vel; } - bool is_accelerating = std::abs(target_vel) > std::abs(current_vel); + // 같은 방향으로 더 빨라지는 경우만 즉시 스냅한다. 부호가 바뀌는 역전은 + // (현재속도가 0이 아닌데 목표가 반대부호로 바뀌는 경우) 저크 제한 감속 + // 경로로 보낸다 — 정지 직전 저속 구간에서 RC/조향 노이즈만으로 목표가 + // 순간 반대부호로 흔들려도 그대로 즉시 스냅되면 "전진→후진→전진"처럼 + // 튀는 현상이 생기고, 실제 반전 요청이라도 0을 관통하는 순간 스냅은 + // 역기전력 스파이크가 오히려 일반 가속보다 더 크다. + bool same_direction = + (current_vel == 0.0f) || (target_vel * current_vel > 0.0f); + bool is_accelerating = + same_direction && (std::abs(target_vel) > std::abs(current_vel)); if (is_accelerating) { current_accel = 0.0f; return target_vel; @@ -1307,6 +1699,14 @@ 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; // 드라이버 알람(과전류/과부하 등) 발생 여부 + // 드라이버 온도(0x20B0)는 열 시정수가 초 단위로 느려 매 틱 통신할 + // 필요가 없다 — 30틱(실측 ~27Hz 기준 약 1.1초)마다만 갱신. + float front_driver_temp_c = 0.0f, rear_driver_temp_c = 0.0f; + int driver_temp_poll_counter = 0; + constexpr int kDriverTempPollTicks = 30; + // 하드웨어 스펙(doc/01) 동작온도 상한 50°C, 드라이버 자체 과열 보호 + // 임계값 기본 80°C 대비 여유를 두고 조기 경보를 띄우는 기준. + constexpr int kOverheatWarnC = 60; 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)) @@ -1329,6 +1729,40 @@ int main(int argc, char **argv) { bool steer_initialized = false, throttle_initialized = false; int steer_reject_streak = 0, throttle_reject_streak = 0; + // IMU Phase 1: 순수 관측용 표시 오프셋. 로봇 전원이 켜질 때(또는 이 + // 프로그램이 시작될 때) IMU가 처음 보고하는 절대 yaw를 "정면(0°)" + // 기준으로 저장해, 화면에는 그 시점 대비 상대 yaw를 보여준다. 이건 + // 순수 표시용 계산일 뿐 제어 로직에는 전혀 관여하지 않는다 — 헤딩을 + // 실제로 잠그거나 보정하는 건 Phase 3의 몫이다. + double imu_yaw_offset = 0.0; + bool imu_yaw_offset_set = false; + + // Phase 2 — 헤딩 적분 + x,y 오도메트리 (여전히 순수 관측, 제어 미개입). + // 처음엔 IMU 내장 AHRS가 계산해 주는 imu_yaw_rel을 θ로 그대로 썼으나, + // 실측 루프백(한 바퀴 돌아 제자리 복귀) 주행 로그로 검증한 결과 그 값이 + // 같은 장치의 원시 자이로(gyro_z)와도 1초 평균 구간 기준 부호 일치율이 + // 72%에 불과함이 드러났다(회전 중 모터 전류로 인한 지자기 간섭 등으로 + // 추정 — 저가 AHRS의 흔한 증상). 실제로 그 루프백 로그에서 odom이 시작 + // 위치로 전혀 돌아오지 못했다. 반면 바퀴 인코더 기반 wheel_omega와 원시 + // gyro_z는 부호 일치율 99.3%로 서로 강하게 신뢰할 수 있음을 같은 로그에서 + // 확인했다. 그래서 θ는 IMU 융합 yaw 대신 wheel_omega와 원시 gyro_z의 + // 평균을 자체 적분한 fused_yaw_deg를 쓴다(IMU 미연결 시엔 wheel_omega만 + // 사용). IMU 융합 yaw(imu_yaw_rel)는 비교용으로 HUD/CSV에 계속 남긴다. + double odom_x = 0.0, odom_y = 0.0; // m, Phase 1 오프셋 설정 시점 기준 + double fused_yaw_deg = 0.0; // deg, wheel_omega+gyro_z 자체 적분 헤딩 + auto last_odom_time = std::chrono::steady_clock::now(); + + // Phase 3 — 헤딩 홀드 PI 상태. 적분 windup 방지용 상한은 트림 상한을 + // Ki로 나눠 "적분항 단독으로도 트림 상한을 넘지 않는" 값으로 고정한다. + bool heading_hold_active_prev = false; + double heading_target_deg = 0.0; + double heading_error_integral = 0.0; + const float heading_integral_limit = + (heading_ki > 1e-6f) ? (heading_max_trim / heading_ki) : 0.0f; + + // Phase 4 — 전신 슬립 디바운스 카운터 + int whole_slip_count = 0; + while (g_running) { // --------------------------------------------------------------------- // RC 수신기 채널 읽기 (doc/joystick.md §2, §3 기능 명세서 기준) @@ -1388,6 +1822,25 @@ int main(int argc, char **argv) { raw_ch[ch] = 0; // 아직 수신 못한 채널은 0으로 표시 } + // IMU Phase 1: 순수 관측. 제어 로직에는 관여하지 않는다. + ImuReader::Snapshot imu_snap = use_imu ? imu.getSnapshot() : ImuReader::Snapshot{}; + if (imu_snap.valid && !imu_yaw_offset_set) { + imu_yaw_offset = imu_snap.angle[2]; + imu_yaw_offset_set = true; + std::cout << "\n[정보] IMU 정면 기준 설정: 시작 시점 yaw " << imu_yaw_offset + << "도를 0도(정면)로 저장했습니다.\n"; + } + double imu_yaw_rel = 0.0; + if (imu_yaw_offset_set) { + imu_yaw_rel = imu_snap.angle[2] - imu_yaw_offset; + // [-180, 180) 범위로 정규화 + while (imu_yaw_rel > 180.0) imu_yaw_rel -= 360.0; + while (imu_yaw_rel < -180.0) imu_yaw_rel += 360.0; + } + // 바퀴 피드백과 무관한 IMU 원시 요레이트. Phase 2(휠-IMU 융합 헤딩)와 + // Phase 4(지령-IMU 전신 슬립 감지, 명령 결정 이전에 필요)가 공유한다. + float imu_omega_rad = imu_snap.gyro[2] * (PI_VAL / 180.0f); // rad/s + if (!rc_ok && prev_rc_ok) { std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms << "ms 이상 미수신) -> 비상 정지.\n"; @@ -1404,6 +1857,7 @@ int main(int argc, char **argv) { float v_x = 0.0f; // 선속도 m/s float omega = 0.0f; // 각속도 rad/s + bool user_steer_centered = false; // Phase 3 헤딩 홀드 진입 조건용 if (!brake_on) { // 3.3 전후진 속도 계산 (CH2) — 실측 결과 raw 값이 높을수록 전진(+), @@ -1438,6 +1892,7 @@ int main(int argc, char **argv) { static_cast(rc_cfg.steer_max - rc_cfg.steer_center_hi)) * omega_max; } + user_steer_centered = (omega == 0.0f); } // --------------------------------------------------------------------- @@ -1445,11 +1900,12 @@ int main(int argc, char **argv) { // (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작) // --------------------------------------------------------------------- std::string lidar_telemetry = "LIDAR:OFF"; + float avoid_omega_offset = 0.0f; // Phase 3 헤딩 홀드가 라이다 회피와 + // 충돌하지 않도록 블록 밖에서도 확인 if (use_lidar) { ObstacleStatus obs = lidar_detector.getStatus(); if (obs.connected) { float speed_scale = 1.0f; - float avoid_omega_offset = 0.0f; // 1. Forward / Backward Automatic Stopping if (v_x > 0.01f) { @@ -1509,6 +1965,68 @@ int main(int argc, char **argv) { } } + // --------------------------------------------------------------------- + // Phase 3 — 직진 헤딩 홀드 (doc/06-imu-integration-plan.md §Phase 3). + // 조향 중립 + 라이다 회피 미개입 + 실제 주행 중일 때만 개입한다. 진입 + // 순간의 fused_yaw_deg를 목표 헤딩으로 고정(lock)하고, 이후 그로부터 + // 벗어난 만큼(오차)을 작은 PI로 보정해 omega에 트림을 더한다. 사용자 + // 조향/정지/라이다 회피가 개입하는 즉시 해제되고 적분항도 리셋된다. + // Phase 2 검증(실측 루프백 로그)에서 wheel_omega/gyro_z 부호 일치율이 + // 99.3%로 확인된 fused_yaw_deg를 피드백으로 쓴다 — IMU 단독 융합 yaw는 + // 회전 중 신뢰도가 낮아 제어 피드백에서 제외했다(imu_yaw_rel은 HUD/CSV + // 비교용으로만 남김). + bool heading_hold_condition = heading_hold_enabled && !brake_on && + user_steer_centered && + avoid_omega_offset == 0.0f && v_x != 0.0f; + float heading_trim_omega = 0.0f; // HUD/CSV 노출용 + if (heading_hold_condition) { + if (!heading_hold_active_prev) { + heading_target_deg = fused_yaw_deg; + heading_error_integral = 0.0; + } + double heading_error_deg = heading_target_deg - fused_yaw_deg; + while (heading_error_deg > 180.0) heading_error_deg -= 360.0; + while (heading_error_deg < -180.0) heading_error_deg += 360.0; + double heading_error_rad = heading_error_deg * (PI_VAL / 180.0); + + heading_error_integral += heading_error_rad * dt; + heading_error_integral = std::clamp( + heading_error_integral, + static_cast(-heading_integral_limit), + static_cast(heading_integral_limit)); + + heading_trim_omega = static_cast( + heading_kp * heading_error_rad + heading_ki * heading_error_integral); + heading_trim_omega = + std::clamp(heading_trim_omega, -heading_max_trim, heading_max_trim); + omega += heading_trim_omega; + } else { + heading_error_integral = 0.0; + } + heading_hold_active_prev = heading_hold_condition; + + // --------------------------------------------------------------------- + // Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (doc §Phase 4). 지령 + // omega(라이다 회피/헤딩 홀드 트림까지 반영된 최종값) 대비 바퀴 피드백과 + // 완전히 독립적인 IMU 원시 gyro_z의 비율이 문턱값 밑으로 + // whole_slip_debounce_ticks 틱 이상 지속되면 "전신 슬립"으로 확정하고, + // 라이다 회피의 speed_scale과 같은 패턴으로 v_x/omega를 일시적으로 + // 낮춘다. 문턱값을 낮게(0.15) 잡아 제자리 회전의 정상적인 스크럽 손실 + // (지령 대비 40~60% 미달도 흔함)과 혼동하지 않고 사실상 전혀 못 도는 + // 극단적 슬립만 잡아낸다. + bool whole_slip_now = false; + if (whole_slip_enabled && imu_snap.valid && + std::abs(omega) > whole_slip_min_omega) { + float slip_ratio = std::abs(imu_omega_rad) / std::abs(omega); + whole_slip_now = slip_ratio < whole_slip_ratio_threshold; + } + whole_slip_count = whole_slip_now ? whole_slip_count + 1 : 0; + bool whole_slip_active = whole_slip_count >= whole_slip_debounce_ticks; + if (whole_slip_active) { + v_x *= whole_slip_speed_scale; + omega *= whole_slip_speed_scale; + } + // 제자리 회전 여부: CH2(전후진) 입력이 완전히 중립(v_x == 0.0f)이면서 // CH1(조향) 입력이 있을 때만 진입한다. v_x는 데드존 처리 시 정확히 // 0.0f로 설정되므로 등호 비교가 안전하다. @@ -1541,14 +2059,22 @@ int main(int argc, char **argv) { // 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 0으로 낮춰 // 헛돌이를 억제한다. 접지력이 회복되어 전류가 정상으로 돌아오면 다음 // 판정 틱에서 자동으로 해제되어 원래 지령으로 복귀한다. - if (airborne_fl) - target_fl = 0.0f; - if (airborne_fr) - target_fr = 0.0f; - if (airborne_rl) - target_rl = 0.0f; - if (airborne_rr) - target_rr = 0.0f; + // 단, 제자리 회전 중에는 반력 토크로 인한 대각선 하중 이동(FL+RR 또는 + // FR+RL 쌍이 동시에 가벼워짐)이 정상적인 현상인데 이걸 "들뜸"으로 오判定해 + // 목표 속도를 꺼버리면 4륜 중 사실상 2륜만 구동되어 회전력이 반토막 + // 난다(실측 로그에서 전체 회전 틱의 38%가 대각선 쌍 동시 들뜸 판정이었고, + // 그 결과 실제 회전율이 지령 대비 44~58%에 그쳤음). 그래서 제자리 회전 + // 중에는 판정(HUD 표시용)은 유지하되 속도를 꺾는 개입만 끈다. + if (!is_spin_turn) { + if (airborne_fl) + target_fl = 0.0f; + if (airborne_fr) + target_fr = 0.0f; + if (airborne_rl) + target_rl = 0.0f; + if (airborne_rr) + target_rr = 0.0f; + } // 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시 // 0으로 스냅 후 브레이크를 잠근다. 해제되면 STOPPED 상태에서 아래 @@ -1647,12 +2173,21 @@ int main(int argc, char **argv) { float fl_amp = 0, fr_amp = 0, rl_amp = 0, rr_amp = 0; int32_t fl_tick = 0, fr_tick = 0, rl_tick = 0, rr_tick = 0; uint16_t err_f_l = 0, err_f_r = 0, err_r_l = 0, err_r_r = 0; + int fl_temp = 0, fr_temp = 0, rl_temp = 0, rr_temp = 0; driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick, fl_amp, fr_amp, - err_f_l, err_f_r); + err_f_l, err_f_r, fl_temp, fr_temp); if (driver_rear_ptr) driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick, rl_amp, - rr_amp, err_r_l, err_r_r); + rr_amp, err_r_l, err_r_r, rl_temp, rr_temp); + + // 드라이버(기판) 온도는 저빈도로만 갱신(위 kDriverTempPollTicks 주석 참고). + if (++driver_temp_poll_counter >= kDriverTempPollTicks) { + driver_temp_poll_counter = 0; + driver_front.readDriverTemp(front_driver_temp_c); + if (driver_rear_ptr) + driver_rear_ptr->readDriverTemp(rear_driver_temp_c); + } // 드라이버 알람(과전류/과부하 등) 감지: 새로 발생한 알람만 콘솔에 // 한 번 크게 출력한다 (매 틱 갱신되는 HUD 줄과 별개의 고정 줄). @@ -1776,6 +2311,40 @@ int main(int argc, char **argv) { float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f); (void)dist_bumper; + // Phase 2 — 휠 기반 ω(엔코더 피드백) vs IMU 기반 ω(자이로) 비교 + + // x,y 오도메트리 적분. 실측 검증(실기 로그) 결과 부호 규약이 일치함: + // 우회전(+omega)일 때 wheel_omega/imu_gyro_z 둘 다 양수로 나온다. + // 이 둘의 차이(잔차)가 §6.3-4의 슬립 지표다. 여전히 제어에는 관여하지 + // 않는다 — HUD/CSV 노출까지만. + auto odom_now = std::chrono::steady_clock::now(); + float odom_dt = std::chrono::duration(odom_now - last_odom_time).count(); + last_odom_time = odom_now; + + float v_l_meas = ((fl_fb + rl_fb) / 2.0f) / rpm_per_ms; // m/s + float v_r_meas = -((fr_fb + rr_fb) / 2.0f) / rpm_per_ms; // m/s (부호 규약: fr/rr는 -v_r로 지령됨) + float v_x_meas = (v_l_meas + v_r_meas) / 2.0f; + float wheel_omega = (k_skid > 0.0f) + ? (v_r_meas - v_l_meas) / (2.0f * k_skid) + : 0.0f; // rad/s + float omega_residual = wheel_omega - imu_omega_rad; + + // θ 적분원: wheel_omega와 원시 gyro_z의 평균(둘의 부호/크기 일치율이 + // 실측상 99.3%로 높음). IMU가 끊겼을 땐 wheel_omega만으로 대체한다. + float fused_omega_rad = + imu_snap.valid ? (wheel_omega + imu_omega_rad) / 2.0f : wheel_omega; + + if (imu_yaw_offset_set && odom_dt > 0.0f && odom_dt < 0.5f) { + // odom_dt>=0.5s: Fail-safe 재연결 등으로 틱이 크게 벌어진 비정상 + // 구간은 위치 적분에서 제외해 순간 도약을 막는다. + fused_yaw_deg += fused_omega_rad * (180.0 / PI_VAL) * odom_dt; + while (fused_yaw_deg > 180.0) fused_yaw_deg -= 360.0; + while (fused_yaw_deg < -180.0) fused_yaw_deg += 360.0; + + double theta_rad = fused_yaw_deg * (PI_VAL / 180.0); + odom_x += v_x_meas * odom_dt * std::cos(theta_rad); + odom_y += v_x_meas * odom_dt * std::sin(theta_rad); + } + if (log_stream.is_open()) { float t_ms = std::chrono::duration( std::chrono::steady_clock::now() - log_start_time) @@ -1786,13 +2355,26 @@ int main(int argc, char **argv) { << 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] << ',' + << (imu_snap.valid ? 1 : 0) << ',' << imu_snap.gyro[2] << ',' + << imu_snap.angle[0] << ',' << imu_snap.angle[1] << ',' + << imu_snap.angle[2] << ',' << imu_yaw_rel << ',' << 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] + << ',' << rl_amp << ',' << rr_amp + << ',' << fl_temp << ',' << fr_temp << ',' << rl_temp << ',' + << rr_temp << ',' << front_driver_temp_c << ',' + << rear_driver_temp_c << ',' << wheel_status_str[0] << ',' << wheel_status_str[1] << ',' << wheel_status_str[2] << ',' << wheel_status_str[3] << ',' << err_f_l << ',' << err_f_r << ',' << err_r_l << ',' << err_r_r << ',' + << fl_tick << ',' << fr_tick << ',' << rl_tick << ',' << rr_tick + << ',' << wheel_omega << ',' << imu_omega_rad << ',' + << omega_residual << ',' << fused_yaw_deg << ',' + << odom_x << ',' << odom_y << ',' + << (heading_hold_condition ? 1 : 0) << ',' << heading_target_deg + << ',' << heading_trim_omega << ',' + << (whole_slip_active ? 1 : 0) << ',' << dist_axle << ',' << comm_ms << '\n'; } @@ -1803,15 +2385,30 @@ int main(int argc, char **argv) { (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 | " + "SPD:%s VX:%+4.2f OMG:%+4.2f | IMU:%s GZ:%+6.2f YAW(imu):%+6.1f | " + "WO:%+5.2f IO:%+5.2f YAW(fus):%+6.1f ODO:(%+5.2f,%+5.2f) | " + "HH:%s TRG:%+6.1f TRM:%+5.2f%s | " + "AXLE:%5.3fm | %-28s | " "I(A):FL%+4.1f FR%+4.1f RL%+4.1f RR%+4.1f | " + "T(C):FL%3d FR%3d RL%3d RR%3d DRV%4.1f/%4.1f%s | " "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, + imu_snap.valid ? "OK " : "LOST", imu_snap.gyro[2], imu_yaw_rel, + wheel_omega, imu_omega_rad, fused_yaw_deg, odom_x, odom_y, + heading_hold_condition ? "ON " : "off", heading_target_deg, + heading_trim_omega, whole_slip_active ? " [!!전신슬립!!]" : "", dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp, - rl_amp, rr_amp, wheel_status_str, - fault_active ? " [!!알람!!]" : "", comm_ms); + rl_amp, rr_amp, fl_temp, fr_temp, rl_temp, rr_temp, + front_driver_temp_c, rear_driver_temp_c, + (fl_temp >= kOverheatWarnC || fr_temp >= kOverheatWarnC || + rl_temp >= kOverheatWarnC || rr_temp >= kOverheatWarnC || + front_driver_temp_c >= kOverheatWarnC || + rear_driver_temp_c >= kOverheatWarnC) + ? " [!!고온!!]" + : "", + wheel_status_str, fault_active ? " [!!알람!!]" : "", comm_ms); fflush(stdout); std::this_thread::sleep_for( @@ -1837,6 +2434,8 @@ int main(int argc, char **argv) { } rc.stop(); + if (use_imu) + imu.stop(); std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n"; return 0; diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp index 1d5e7e7..d06e512 100755 Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ