diff --git a/Makefile b/Makefile
index f05487e..c934c91 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 CXX = g++
-CXXFLAGS = -O3 -std=c++17 -Wall -Wextra
-LIBS = -lSDL2 -lpthread
+CXXFLAGS = -O3 -std=c++17 -Wall -Wextra -I/usr/local/include
+LIBS = -lSDL2 -lpthread -llivox_lidar_sdk_shared
 
 TARGET = xbox_motor_control_cpp
 SRC = xbox_motor_control.cpp
@@ -8,7 +8,7 @@ SRC = xbox_motor_control.cpp
 all: $(TARGET)
 
 $(TARGET): $(SRC)
-	$(CXX) $(CXXFLAGS) $(SRC) $(LIBS) -o $(TARGET)
+	$(CXX) $(CXXFLAGS) $(SRC) -o $(TARGET) $(LIBS)
 
 clean:
 	rm -f $(TARGET)
diff --git a/mid360s_config.json b/mid360s_config.json
new file mode 100644
index 0000000..fb0e54c
--- /dev/null
+++ b/mid360s_config.json
@@ -0,0 +1,21 @@
+{
+  "Mid360s": {
+    "lidar_net_info" : {
+      "cmd_data_port"  : 56100,
+      "push_msg_port"  : 56200,
+      "point_data_port": 56300,
+      "imu_data_port"  : 56400,
+      "log_data_port"  : 56500
+    },
+    "host_net_info" : [
+      {
+        "host_ip"        : "192.168.1.5",
+        "cmd_data_port"  : 56101,
+        "push_msg_port"  : 56201,
+        "point_data_port": 56301,
+        "imu_data_port"  : 56401,
+        "log_data_port"  : 56501
+      }
+    ]
+  }
+}
diff --git a/run_4wd.sh b/run_4wd.sh
index e0ca132..bb49520 100755
--- a/run_4wd.sh
+++ b/run_4wd.sh
@@ -1,13 +1,13 @@
 #!/usr/bin/env bash
 
-# 1-Click Launch Script for C++ 4WD Motor Control
+# 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance
 PORT="${1:-/dev/ttyUSB0}"
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 
 cd "$SCRIPT_DIR" || exit 1
 
 echo "=================================================================="
-echo " 🚀 ZLAC8015D 4WD C++ 초저지연 모터 제어 원클릭 런치 시스템"
+echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템"
 echo "=================================================================="
 
 # 1. USB Latency 1ms 단축
@@ -23,8 +23,14 @@ if [ ! -f "./xbox_motor_control_cpp" ]; then
 fi
 
 echo "=================================================================="
-echo " [시작] C++ 100Hz 초저지연 4WD 조이스틱 제어 시작 ($PORT)"
+echo " [시작] C++ 100Hz 초저지연 4WD 조이스틱 + 라이다 장애물 회피 시작 ($PORT)"
+echo "  - 라이다 기능 토글: Xbox 컨트롤러 X 버튼"
+echo "  - 정지/종료: B 버튼 또는 Ctrl+C"
 echo "=================================================================="
 
-# 3. C++ 4WD 프로그램 실행
-./xbox_motor_control_cpp --port "$PORT"
+# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능)
+if [ $# -gt 1 ]; then
+    ./xbox_motor_control_cpp --port "$PORT" "${@:2}"
+else
+    ./xbox_motor_control_cpp --port "$PORT"
+fi
diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp
index de2234c..ffbf1ce 100644
--- a/xbox_motor_control.cpp
+++ b/xbox_motor_control.cpp
@@ -1,549 +1,1208 @@
-#include <iostream>
-#include <vector>
-#include <string>
-#include <chrono>
-#include <thread>
-#include <cmath>
+#include <SDL2/SDL.h>
 #include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <cmath>
 #include <csignal>
 #include <fcntl.h>
-#include <termios.h>
-#include <unistd.h>
+#include <fstream>
+#include <iostream>
+#include <mutex>
+#include <string>
 #include <sys/ioctl.h>
-#include <SDL2/SDL.h>
+#include <termios.h>
+#include <thread>
+#include <unistd.h>
+#include <vector>
+
+#include "livox_lidar_api.h"
+#include "livox_lidar_def.h"
 
 // Global flag for signal handling
 volatile std::sig_atomic_t g_running = 1;
 
 void signalHandler(int signum) {
-    (void)signum;
-    g_running = 0;
+  (void)signum;
+  g_running = 0;
 }
 
 // Modbus RTU CRC16 calculation
-uint16_t calcCRC16(const uint8_t* data, size_t len) {
-    uint16_t crc = 0xFFFF;
-    for (size_t i = 0; i < len; ++i) {
-        crc ^= data[i];
-        for (int j = 0; j < 8; ++j) {
-            if (crc & 0x0001) {
-                crc >>= 1;
-                crc ^= 0xA001;
-            } else {
-                crc >>= 1;
-            }
-        }
+uint16_t calcCRC16(const uint8_t *data, size_t len) {
+  uint16_t crc = 0xFFFF;
+  for (size_t i = 0; i < len; ++i) {
+    crc ^= data[i];
+    for (int j = 0; j < 8; ++j) {
+      if (crc & 0x0001) {
+        crc >>= 1;
+        crc ^= 0xA001;
+      } else {
+        crc >>= 1;
+      }
     }
-    return crc;
+  }
+  return crc;
 }
 
 class SerialPort {
 private:
-    int fd_;
-    std::string port_name_;
+  int fd_;
+  std::string port_name_;
 
 public:
-    SerialPort() : fd_(-1) {}
-    ~SerialPort() { closePort(); }
+  SerialPort() : fd_(-1) {}
+  ~SerialPort() { closePort(); }
 
-    bool openPort(const std::string& port_name, int baudrate = 115200) {
-        port_name_ = port_name;
-        fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
-        if (fd_ < 0) {
-            std::cerr << "[오류] C++ 포트 열기 실패: " << port_name << std::endl;
-            return false;
-        }
-
-        fcntl(fd_, F_SETFL, 0);
-
-        struct termios options;
-        tcgetattr(fd_, &options);
-
-        speed_t speed = B115200;
-        switch (baudrate) {
-            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;   // No parity
-        options.c_cflag &= ~CSTOPB;   // 1 stop bit
-        options.c_cflag &= ~CSIZE;
-        options.c_cflag |= CS8;       // 8 data bits
-        options.c_cflag &= ~CRTSCTS;  // No hardware flow control
-        options.c_cflag |= CREAD | CLOCAL;
-
-        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
-        options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
-        options.c_oflag &= ~OPOST;
-
-        options.c_cc[VMIN] = 0;
-        options.c_cc[VTIME] = 1;  // 0.1s timeout
-
-        tcsetattr(fd_, TCSANOW, &options);
-        tcflush(fd_, TCIOFLUSH);
-        return true;
+  bool openPort(const std::string &port_name, int baudrate = 115200) {
+    port_name_ = port_name;
+    fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd_ < 0) {
+      std::cerr << "[오류] C++ 포트 열기 실패: " << port_name << std::endl;
+      return false;
     }
 
-    void closePort() {
-        if (fd_ >= 0) {
-            close(fd_);
-            fd_ = -1;
-        }
+    fcntl(fd_, F_SETFL, 0);
+
+    struct termios options;
+    tcgetattr(fd_, &options);
+
+    speed_t speed = B115200;
+    switch (baudrate) {
+    case 9600:
+      speed = B9600;
+      break;
+    case 19200:
+      speed = B19200;
+      break;
+    case 38400:
+      speed = B38400;
+      break;
+    case 57600:
+      speed = B57600;
+      break;
+    default:
+      speed = B115200;
+      break;
     }
 
-    bool writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
-        uint8_t pkt[8];
-        pkt[0] = slave;
-        pkt[1] = 0x06;
-        pkt[2] = (reg >> 8) & 0xFF;
-        pkt[3] = reg & 0xFF;
-        pkt[4] = (val >> 8) & 0xFF;
-        pkt[5] = val & 0xFF;
+    cfsetispeed(&options, speed);
+    cfsetospeed(&options, speed);
 
-        uint16_t crc = calcCRC16(pkt, 6);
-        pkt[6] = crc & 0xFF;
-        pkt[7] = (crc >> 8) & 0xFF;
+    options.c_cflag &= ~PARENB; // No parity
+    options.c_cflag &= ~CSTOPB; // 1 stop bit
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8;      // 8 data bits
+    options.c_cflag &= ~CRTSCTS; // No hardware flow control
+    options.c_cflag |= CREAD | CLOCAL;
 
-        tcflush(fd_, TCIOFLUSH);
-        ssize_t written = write(fd_, pkt, 8);
-        if (written != 8) return false;
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
+                         ISTRIP | INLCR | IGNCR | ICRNL);
+    options.c_oflag &= ~OPOST;
 
-        if (slave == 0) return true; // Broadcast
+    options.c_cc[VMIN] = 0;
+    options.c_cc[VTIME] = 1; // 0.1s timeout
 
-        uint8_t rx_buf[8];
-        ssize_t rx_bytes = 0;
-        auto start = std::chrono::steady_clock::now();
-        while (rx_bytes < 8) {
-            ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
-            if (res > 0) rx_bytes += res;
-            auto now = std::chrono::steady_clock::now();
-            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50) break;
-            std::this_thread::sleep_for(std::chrono::microseconds(500));
-        }
-        return rx_bytes == 8;
+    tcsetattr(fd_, TCSANOW, &options);
+    tcflush(fd_, TCIOFLUSH);
+    return true;
+  }
+
+  void closePort() {
+    if (fd_ >= 0) {
+      close(fd_);
+      fd_ = -1;
+    }
+  }
+
+  bool writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
+    uint8_t pkt[8];
+    pkt[0] = slave;
+    pkt[1] = 0x06;
+    pkt[2] = (reg >> 8) & 0xFF;
+    pkt[3] = reg & 0xFF;
+    pkt[4] = (val >> 8) & 0xFF;
+    pkt[5] = val & 0xFF;
+
+    uint16_t crc = calcCRC16(pkt, 6);
+    pkt[6] = crc & 0xFF;
+    pkt[7] = (crc >> 8) & 0xFF;
+
+    tcflush(fd_, TCIOFLUSH);
+    ssize_t written = write(fd_, pkt, 8);
+    if (written != 8)
+      return false;
+
+    if (slave == 0)
+      return true; // Broadcast
+
+    uint8_t rx_buf[8];
+    ssize_t rx_bytes = 0;
+    auto start = std::chrono::steady_clock::now();
+    while (rx_bytes < 8) {
+      ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
+      if (res > 0)
+        rx_bytes += res;
+      auto now = std::chrono::steady_clock::now();
+      if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
+              .count() > 50)
+        break;
+      std::this_thread::sleep_for(std::chrono::microseconds(500));
+    }
+    if (rx_bytes != 8)
+      return false;
+
+    // CRC 검증: 손상된 응답을 정상으로 오인하지 않도록 확인
+    uint16_t resp_crc = calcCRC16(rx_buf, 6);
+    uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) |
+                        (static_cast<uint16_t>(rx_buf[7]) << 8);
+    return resp_crc == recv_crc && rx_buf[0] == slave;
+  }
+
+  bool writeRegs(uint8_t slave, uint16_t reg,
+                 const std::vector<uint16_t> &vals) {
+    size_t count = vals.size();
+    size_t pkt_len = 7 + count * 2 + 2;
+    std::vector<uint8_t> pkt(pkt_len);
+
+    pkt[0] = slave;
+    pkt[1] = 0x10;
+    pkt[2] = (reg >> 8) & 0xFF;
+    pkt[3] = reg & 0xFF;
+    pkt[4] = (count >> 8) & 0xFF;
+    pkt[5] = count & 0xFF;
+    pkt[6] = static_cast<uint8_t>(count * 2);
+
+    for (size_t i = 0; i < count; ++i) {
+      pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
+      pkt[8 + i * 2] = vals[i] & 0xFF;
     }
 
-    bool writeRegs(uint8_t slave, uint16_t reg, const std::vector<uint16_t>& vals) {
-        size_t count = vals.size();
-        size_t pkt_len = 7 + count * 2 + 2;
-        std::vector<uint8_t> pkt(pkt_len);
+    uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
+    pkt[pkt_len - 2] = crc & 0xFF;
+    pkt[pkt_len - 1] = (crc >> 8) & 0xFF;
 
-        pkt[0] = slave;
-        pkt[1] = 0x10;
-        pkt[2] = (reg >> 8) & 0xFF;
-        pkt[3] = reg & 0xFF;
-        pkt[4] = (count >> 8) & 0xFF;
-        pkt[5] = count & 0xFF;
-        pkt[6] = static_cast<uint8_t>(count * 2);
+    tcflush(fd_, TCIOFLUSH);
+    ssize_t written = write(fd_, pkt.data(), pkt_len);
+    if (written != static_cast<ssize_t>(pkt_len))
+      return false;
 
-        for (size_t i = 0; i < count; ++i) {
-            pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
-            pkt[8 + i * 2] = vals[i] & 0xFF;
-        }
+    if (slave == 0)
+      return true;
 
-        uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
-        pkt[pkt_len - 2] = crc & 0xFF;
-        pkt[pkt_len - 1] = (crc >> 8) & 0xFF;
+    uint8_t rx_buf[8];
+    ssize_t rx_bytes = 0;
+    auto start = std::chrono::steady_clock::now();
+    while (rx_bytes < 8) {
+      ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
+      if (res > 0)
+        rx_bytes += res;
+      auto now = std::chrono::steady_clock::now();
+      if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
+              .count() > 50)
+        break;
+      std::this_thread::sleep_for(std::chrono::microseconds(500));
+    }
+    if (rx_bytes != 8)
+      return false;
 
-        tcflush(fd_, TCIOFLUSH);
-        ssize_t written = write(fd_, pkt.data(), pkt_len);
-        if (written != static_cast<ssize_t>(pkt_len)) return false;
+    uint16_t resp_crc = calcCRC16(rx_buf, 6);
+    uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) |
+                        (static_cast<uint16_t>(rx_buf[7]) << 8);
+    return resp_crc == recv_crc && rx_buf[0] == slave;
+  }
 
-        if (slave == 0) return true;
+  bool readRegs(uint8_t slave, uint16_t reg, uint16_t count,
+                std::vector<uint16_t> &out_vals) {
+    uint8_t pkt[8];
+    pkt[0] = slave;
+    pkt[1] = 0x03;
+    pkt[2] = (reg >> 8) & 0xFF;
+    pkt[3] = reg & 0xFF;
+    pkt[4] = (count >> 8) & 0xFF;
+    pkt[5] = count & 0xFF;
 
-        uint8_t rx_buf[8];
-        ssize_t rx_bytes = 0;
-        auto start = std::chrono::steady_clock::now();
-        while (rx_bytes < 8) {
-            ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
-            if (res > 0) rx_bytes += res;
-            auto now = std::chrono::steady_clock::now();
-            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50) break;
-            std::this_thread::sleep_for(std::chrono::microseconds(500));
-        }
-        return rx_bytes == 8;
+    uint16_t crc = calcCRC16(pkt, 6);
+    pkt[6] = crc & 0xFF;
+    pkt[7] = (crc >> 8) & 0xFF;
+
+    tcflush(fd_, TCIOFLUSH);
+    if (write(fd_, pkt, 8) != 8)
+      return false;
+
+    size_t expected_bytes = 5 + count * 2;
+    std::vector<uint8_t> rx_buf(expected_bytes);
+    size_t rx_bytes = 0;
+
+    auto start = std::chrono::steady_clock::now();
+    while (rx_bytes < expected_bytes) {
+      ssize_t res =
+          read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
+      if (res > 0)
+        rx_bytes += res;
+      auto now = std::chrono::steady_clock::now();
+      // 15ms -> 25ms: 100Hz(10ms 주기) 루프 내에서 응답을 안정적으로 받기 위한
+      // 여유 확보
+      if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
+              .count() > 25)
+        break;
+      std::this_thread::sleep_for(std::chrono::microseconds(100));
     }
 
-    bool readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector<uint16_t>& out_vals) {
-        uint8_t pkt[8];
-        pkt[0] = slave;
-        pkt[1] = 0x03;
-        pkt[2] = (reg >> 8) & 0xFF;
-        pkt[3] = reg & 0xFF;
-        pkt[4] = (count >> 8) & 0xFF;
-        pkt[5] = count & 0xFF;
-
-        uint16_t crc = calcCRC16(pkt, 6);
-        pkt[6] = crc & 0xFF;
-        pkt[7] = (crc >> 8) & 0xFF;
-
-        tcflush(fd_, TCIOFLUSH);
-        if (write(fd_, pkt, 8) != 8) return false;
-
-        size_t expected_bytes = 5 + count * 2;
-        std::vector<uint8_t> rx_buf(expected_bytes);
-        size_t rx_bytes = 0;
-
-        auto start = std::chrono::steady_clock::now();
-        while (rx_bytes < expected_bytes) {
-            ssize_t res = read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
-            if (res > 0) rx_bytes += res;
-            auto now = std::chrono::steady_clock::now();
-            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 15) break;
-            std::this_thread::sleep_for(std::chrono::microseconds(100));
-        }
-
-        if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
-            out_vals.resize(count);
-            for (uint16_t i = 0; i < count; ++i) {
-                out_vals[i] = (static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
-            }
-            return true;
-        }
+    if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
+      // CRC 검증 추가: 노이즈로 손상된 응답을 정상 데이터로 오인하지 않도록
+      // 방지
+      uint16_t resp_crc = calcCRC16(rx_buf.data(), expected_bytes - 2);
+      uint16_t recv_crc =
+          static_cast<uint16_t>(rx_buf[expected_bytes - 2]) |
+          (static_cast<uint16_t>(rx_buf[expected_bytes - 1]) << 8);
+      if (resp_crc != recv_crc)
         return false;
+
+      out_vals.resize(count);
+      for (uint16_t i = 0; i < count; ++i) {
+        out_vals[i] =
+            (static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
+      }
+      return true;
     }
+    return false;
+  }
 };
 
 class MotorDriver {
 private:
-    SerialPort* port_;
-    uint8_t slave_id_;
+  SerialPort *port_;
+  uint8_t slave_id_;
 
 public:
-    MotorDriver(SerialPort* port, uint8_t slave_id) : port_(port), slave_id_(slave_id) {}
+  MotorDriver(SerialPort *port, uint8_t slave_id)
+      : port_(port), slave_id_(slave_id) {}
 
-    bool initDriver(uint16_t acl_ms = 500, uint16_t dcl_ms = 500) {
-        port_->writeReg(slave_id_, 0x200E, 0x0006); // Alarm clear
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
-        port_->writeReg(slave_id_, 0x200E, 0x0007); // Disable
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
-        port_->writeReg(slave_id_, 0x200D, 3);      // Velocity mode
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
+  bool initDriver(uint16_t acl_ms = 500, uint16_t dcl_ms = 500) {
+    port_->writeReg(slave_id_, 0x200E, 0x0006); // Alarm clear
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    port_->writeReg(slave_id_, 0x200E, 0x0007); // Disable
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    port_->writeReg(slave_id_, 0x200D, 3); // Velocity mode
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
 
-        port_->writeRegs(slave_id_, 0x2080, {acl_ms, acl_ms}); // Accel
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
-        port_->writeRegs(slave_id_, 0x2082, {dcl_ms, dcl_ms}); // Decel
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    port_->writeRegs(slave_id_, 0x2080, {acl_ms, acl_ms}); // Accel
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    port_->writeRegs(slave_id_, 0x2082, {dcl_ms, dcl_ms}); // Decel
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
 
-        port_->writeReg(slave_id_, 0x200E, 0x0008); // Enable
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
-        setBrakes(true);
-        return true;
-    }
-
-    void setBrakes(bool lock) {
-        uint16_t val = lock ? 1 : 0;
-        port_->writeReg(slave_id_, 0x201A, val);
-        port_->writeReg(slave_id_, 0x201B, val);
-    }
-
-    void setRPMs(float l_rpm, float r_rpm) {
-        int16_t l_val = static_cast<int16_t>(std::clamp(l_rpm, -3000.0f, 3000.0f));
-        int16_t r_val = static_cast<int16_t>(std::clamp(r_rpm, -3000.0f, 3000.0f));
-        port_->writeRegs(slave_id_, 0x2088, {static_cast<uint16_t>(l_val), static_cast<uint16_t>(r_val)});
-    }
-
-    bool readFeedback(float& l_fb, float& r_fb, int32_t& l_tick, int32_t& r_tick) {
-        std::vector<uint16_t> regs;
-        if (port_->readRegs(slave_id_, 0x20A7, 6, regs)) {
-            uint32_t val_l = ((static_cast<uint32_t>(regs[0]) & 0xFFFF) << 16) | (regs[1] & 0xFFFF);
-            l_tick = (val_l < 0x80000000U) ? static_cast<int32_t>(val_l) : static_cast<int32_t>(val_l - 0x100000000ULL);
-
-            uint32_t val_r = ((static_cast<uint32_t>(regs[2]) & 0xFFFF) << 16) | (regs[3] & 0xFFFF);
-            r_tick = (val_r < 0x80000000U) ? static_cast<int32_t>(val_r) : static_cast<int32_t>(val_r - 0x100000000ULL);
-
-            int16_t vl = static_cast<int16_t>(regs[4]);
-            int16_t vr = static_cast<int16_t>(regs[5]);
-            l_fb = static_cast<float>(vl);
-            r_fb = static_cast<float>(vr);
-            return true;
-        }
-        return false;
+    port_->writeReg(slave_id_, 0x200E, 0x0008); // Enable
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    setBrakes(true);
+    return true;
+  }
+
+  void setBrakes(bool lock) {
+    uint16_t val = lock ? 1 : 0;
+    port_->writeReg(slave_id_, 0x201A, val);
+    port_->writeReg(slave_id_, 0x201B, val);
+  }
+
+  void setRPMs(float l_rpm, float r_rpm) {
+    int16_t l_val = static_cast<int16_t>(std::clamp(l_rpm, -3000.0f, 3000.0f));
+    int16_t r_val = static_cast<int16_t>(std::clamp(r_rpm, -3000.0f, 3000.0f));
+    port_->writeRegs(
+        slave_id_, 0x2088,
+        {static_cast<uint16_t>(l_val), static_cast<uint16_t>(r_val)});
+  }
+
+  bool readFeedback(float &l_fb, float &r_fb, int32_t &l_tick,
+                    int32_t &r_tick, float &l_torque_a, float &r_torque_a) {
+    std::vector<uint16_t> regs;
+    // 0x20A7~0x20AE 8레지스터 연속 읽기: 포지션 틱, RPM 피드백에 이어
+    // 실제 토크(전류, 0x20AD/0x20AE)까지 한 번의 통신으로 확보.
+    // 무부하(공중에 뜬) 바퀴는 전류가 급격히 낮아지므로 슬립 진단에 사용.
+    if (port_->readRegs(slave_id_, 0x20A7, 8, regs)) {
+      // 수정: uint32_t -> int32_t 캐스팅은 2의 보수 표현에서 안전하게 부호가
+      // 재해석됨. 기존의 "val - 0x100000000ULL" 방식은 uint64_t 승격 후
+      // int32_t로 축소 캐스팅하는 구현정의 동작(현실적으로는 대부분 동작하지만
+      // 명확성/이식성이 떨어짐)이라 제거함.
+      uint32_t val_l = (static_cast<uint32_t>(regs[0]) << 16) | regs[1];
+      l_tick = static_cast<int32_t>(val_l);
+
+      uint32_t val_r = (static_cast<uint32_t>(regs[2]) << 16) | regs[3];
+      r_tick = static_cast<int32_t>(val_r);
+
+      int16_t vl = static_cast<int16_t>(regs[4]);
+      int16_t vr = static_cast<int16_t>(regs[5]);
+      l_fb = static_cast<float>(vl) * 0.1f; // 0.1RPM 단위 -> RPM
+      r_fb = static_cast<float>(vr) * 0.1f;
+
+      int16_t tl = static_cast<int16_t>(regs[6]);
+      int16_t tr = static_cast<int16_t>(regs[7]);
+      l_torque_a = static_cast<float>(tl) * 0.1f; // 0.1A 단위 -> A
+      r_torque_a = static_cast<float>(tr) * 0.1f;
+      return true;
     }
+    return false;
+  }
 };
 
-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);
-    // 선형 커브 (Linear Curve): 토크 저하 없이 스틱 조작에 100% 즉각 출력을 보장
-    return sign * norm;
-}
+// -----------------------------------------------------------------------------
+// Livox Mid-360S Single LiDAR Obstacle Detection & Avoidance Class
+// -----------------------------------------------------------------------------
+struct ObstacleStatus {
+  float min_dist_front = 999.0f; // m
+  float min_dist_rear = 999.0f;  // m
+  float min_dist_left = 999.0f;  // m
+  float min_dist_right = 999.0f; // m
+  bool connected = false;
+  std::chrono::steady_clock::time_point last_update;
+};
 
-int main(int argc, char** argv) {
-    std::signal(SIGINT, signalHandler);
-    std::signal(SIGTERM, signalHandler);
+class LidarObstacleDetector {
+private:
+  std::string config_path_;
+  std::atomic<bool> initialized_{false};
+  std::mutex status_mutex_;
+  ObstacleStatus status_;
 
-    std::string port1 = "/dev/ttyUSB0";
-    std::string port2 = "";
-    uint8_t id1 = 1;
-    uint8_t id2 = 2;
-    bool bcast_mode = false;
-    float max_v = 0.30f;          // m/s
-    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인치 휠 반지름 (131.517mm)
-    float accel_rate = 120.0f;    // RPM/s
-    float decel_rate = 180.0f;    // RPM/s (역기전력 서지 방지 및 소프트 정지 튜닝)
-    float bumper_offset = 0.045f; // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리 (45mm)
+  // Filtering & ROI parameters
+  float min_z_ = -0.40f; // m (Z axis relative to LiDAR after tilt rotation)
+  float max_z_ = 0.80f;  // m
+  float min_r_ = 0.15f;  // m (blind spot / chassis)
+  float max_r_ = 3.50f;  // m (max range)
+  float robot_half_width_ =
+      0.255f; // m (robot width 0.410m / 2 + 0.05m margin = 0.255m)
+  float pitch_deg_ = 17.0f;     // deg (LiDAR downward tilt angle)
+  float lidar_height_ = 0.460f; // m (LiDAR height above ground)
+  float lidar_x_offset_ =
+      0.250f; // m (LiDAR forward offset 250mm from robot center)
 
-    for (int i = 1; i < argc; ++i) {
-        std::string arg = argv[i];
-        if (arg == "--port" && i + 1 < argc) port1 = argv[++i];
-        else if (arg == "--port1" && i + 1 < argc) port1 = argv[++i];
-        else if (arg == "--port2" && i + 1 < argc) port2 = argv[++i];
-        else if (arg == "--id1" && i + 1 < argc) id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
-        else if (arg == "--id2" && i + 1 < argc) id2 = static_cast<uint8_t>(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 == "--radius" && i + 1 < argc) wheel_radius = std::stof(argv[++i]);
-        else if (arg == "--bumper" && i + 1 < argc) 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) decel_rate = std::stof(argv[++i]);
-        else if (arg == "--bcast" || arg == "--broadcast") bcast_mode = true;
+  float cos_pitch_ = std::cos(17.0f * 3.14159265f / 180.0f);
+  float sin_pitch_ = std::sin(17.0f * 3.14159265f / 180.0f);
+
+public:
+  static LidarObstacleDetector *instance_;
+
+  LidarObstacleDetector() { instance_ = this; }
+
+  ~LidarObstacleDetector() { stop(); }
+
+  void setParams(float min_z, float max_z, float robot_half_width,
+                 float pitch_deg = 17.0f, float lidar_height = 0.460f,
+                 float lidar_x_offset = 0.250f) {
+    min_z_ = min_z;
+    max_z_ = max_z;
+    robot_half_width_ = robot_half_width;
+    pitch_deg_ = pitch_deg;
+    lidar_height_ = lidar_height;
+    lidar_x_offset_ = lidar_x_offset;
+
+    float pitch_rad = pitch_deg_ * 3.14159265358979323846f / 180.0f;
+    cos_pitch_ = std::cos(pitch_rad);
+    sin_pitch_ = std::sin(pitch_rad);
+  }
+
+  bool init(const std::string &config_path) {
+    config_path_ = config_path;
+    if (!LivoxLidarSdkInit(config_path_.c_str())) {
+      std::cerr << "[경고] Livox Mid-360S SDK2 초기화 실패 (" << config_path_
+                << ")\n";
+      return false;
     }
 
-    constexpr float PI_VAL = 3.14159265358979323846f;
-    float rpm_per_ms = 60.0f / (2.0f * PI_VAL * wheel_radius);
-    
-    // 4WD 유효 선회 계수 (Track Width 576mm, Wheelbase 368.4mm 정밀 기하학)
-    float effective_w = std::sqrt(track_width * track_width + wheelbase * wheelbase);
-    float k_skid = (track_width + (wheelbase * wheelbase) / track_width) / 2.0f;
+    SetLivoxLidarPointCloudCallBack(PointCloudCallbackStatic, nullptr);
+    SetLivoxLidarInfoChangeCallback(InfoChangeCallbackStatic, nullptr);
 
-    std::cout << "\n==================================================================\n";
-    std::cout << " [C++17 4WD 정밀 직진/감속 보정] ZLAC8015D 4모터 동기화 시스템\n";
-    std::cout << "  - 좌우 바퀴 거리 (Track Width): " << track_width * 1000.0f << " mm\n";
-    std::cout << "  - 전후 바퀴 거리 (Wheelbase)  : " << wheelbase * 1000.0f << " mm\n";
-    std::cout << "  - 범퍼 오프셋 (Bumper Offset) : " << bumper_offset * 1000.0f << " mm\n";
-    std::cout << "  - 실시간 직진 엔코더 자동 수평 보정 (Straight Auto-Correction) 활성화\n";
-    std::cout << "==================================================================\n";
+    initialized_ = true;
+    std::cout << "[정보] Livox Mid-360S 라이다 수신 시작 (설정: "
+              << config_path_ << ", 피치 기울임: " << pitch_deg_
+              << "도, 설치높이: " << lidar_height_ * 1000.0f
+              << "mm, 전방 오프셋: " << lidar_x_offset_ * 1000.0f << "mm)\n";
+    return true;
+  }
 
-    if (SDL_Init(SDL_INIT_JOYSTICK) < 0) {
-        std::cerr << "[오류] SDL 조이스틱 초기화 실패: " << SDL_GetError() << std::endl;
-        return 1;
+  void stop() {
+    if (initialized_) {
+      LivoxLidarSdkUninit();
+      initialized_ = false;
+      std::cout << "[정보] Livox Mid-360S 라이다 수신기 종료.\n";
     }
+  }
 
-    if (SDL_NumJoysticks() < 1) {
-        std::cerr << "[경고] 무선 Xbox 컨트롤러(조이스틱)가 연결되지 않았습니다.\n";
-        SDL_Quit();
-        return 1;
-    }
+  static void PointCloudCallbackStatic(const uint32_t handle,
+                                       const uint8_t dev_type,
+                                       LivoxLidarEthernetPacket *data,
+                                       void *client_data) {
+    (void)dev_type;
+    (void)client_data;
+    if (instance_)
+      instance_->handlePointCloud(handle, data);
+  }
 
-    SDL_Joystick* joystick = SDL_JoystickOpen(0);
-    if (!joystick) {
-        std::cerr << "[오류] 조이스틱 열기 실패!\n";
-        SDL_Quit();
-        return 1;
-    }
-    std::cout << "[정보] C++ Xbox 컨트롤러 연결 성공: " << SDL_JoystickName(joystick) << std::endl;
+  static void InfoChangeCallbackStatic(const uint32_t handle,
+                                       const LivoxLidarInfo *info,
+                                       void *client_data) {
+    (void)client_data;
+    if (!info)
+      return;
+    std::cout << "\n[라이다 연결 완료] Handle: " << handle
+              << ", SN: " << info->sn << ", IP: " << info->lidar_ip
+              << std::endl;
+    SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr);
+  }
 
-    SerialPort sp1, sp2;
-    if (!sp1.openPort(port1)) return 1;
-    std::cout << "[정보] RS485 포트1 (" << port1 << ") 오픈 성공.\n";
+  void handlePointCloud(uint32_t handle, LivoxLidarEthernetPacket *packet) {
+    (void)handle;
+    if (!packet)
+      return;
 
-    SerialPort* sp2_ptr = &sp1;
-    if (!port2.empty()) {
-        if (sp2.openPort(port2)) {
-            sp2_ptr = &sp2;
-            std::cout << "[정보] RS485 포트2 (" << port2 << ") 오픈 성공.\n";
+    float curr_front = 999.0f;
+    float curr_rear = 999.0f;
+    float curr_left = 999.0f;
+    float curr_right = 999.0f;
+
+    auto parsePoint = [&](float raw_x, float raw_y, float raw_z) {
+      // Apply 17 degree pitch rotation (LiDAR tilted downward towards ground)
+      float x = raw_x * cos_pitch_ + raw_z * sin_pitch_;
+      float y = raw_y;
+      float z = -raw_x * sin_pitch_ + raw_z * cos_pitch_;
+
+      if (z < min_z_ || z > max_z_)
+        return; // Filter ground floor and ceiling
+      float r = std::sqrt(x * x + y * y);
+      if (r < min_r_ || r > max_r_)
+        return;
+
+      // Coordinate Frame: +X Forward, +Y Left, +Z Up
+      // 1. Front Obstacle Zone
+      if (x > 0.05f && std::abs(y) <= robot_half_width_) {
+        if (x < curr_front)
+          curr_front = x;
+      }
+      // 2. Rear Obstacle Zone
+      else if (x < -0.05f && std::abs(y) <= robot_half_width_) {
+        if (-x < curr_rear)
+          curr_rear = -x;
+      }
+
+      // 3. Side Obstacle Zones (Front Lookahead window: X in [0.05m, 1.8m])
+      if (x > 0.05f && x < 1.8f) {
+        if (y > 0.10f && y < 1.2f) { // Left Side
+          float dist = std::sqrt(x * x + y * y);
+          if (dist < curr_left)
+            curr_left = dist;
+        } else if (y < -0.10f && y > -1.2f) { // Right Side
+          float dist = std::sqrt(x * x + y * y);
+          if (dist < curr_right)
+            curr_right = dist;
         }
-    }
-
-    MotorDriver driver_front(&sp1, bcast_mode ? 0 : id1);
-    driver_front.initDriver(150, 150);
-
-    MotorDriver* driver_rear_ptr = nullptr;
-    MotorDriver driver_rear(sp2_ptr, bcast_mode ? 0 : id2);
-    if (!bcast_mode) {
-        driver_rear.initDriver(150, 150);
-        driver_rear_ptr = &driver_rear;
-    }
-
-    std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 또는 Ctrl+C)\n\n";
-
-    float target_fl = 0.0f, target_fr = 0.0f;
-    float target_rl = 0.0f, target_rr = 0.0f;
-
-    float cmd_fl = 0.0f, cmd_fr = 0.0f;
-    float cmd_rl = 0.0f, cmd_rr = 0.0f;
-
-    const float loop_hz = 100.0f;
-    const float dt = 1.0f / loop_hz;
-    const float max_accel_step = accel_rate * dt;
-    const float max_decel_step = decel_rate * dt;
-
-    std::string state = "STOPPED";
-    bool trip_initialized = false;
-    int32_t start_fl = 0, start_fr = 0, start_rl = 0, start_rr = 0;
-
-    // S-Curve 부드러운 가속 보정 람다 함수 (좌/우 모터 대칭 가속 보장)
-    auto sCurveStep = [](float current, float target, float max_accel, float max_decel) {
-        float diff = target - current;
-        if (std::abs(diff) < 0.01f) return target;
-
-        // 속도 절댓값 크기가 커지는 중이면 가속(accel), 줄어드는 중이면 감속(decel) 적용
-        bool is_accelerating = std::abs(target) > std::abs(current);
-        float step_rate = is_accelerating ? max_accel : max_decel;
-        float step = (diff > 0.0f) ? step_rate : -step_rate;
-
-        float ratio = std::clamp(std::abs(diff) / 60.0f, 0.15f, 1.0f);
-        float smooth = ratio * ratio * (3.0f - 2.0f * ratio);
-        float delta = step * smooth;
-        if (std::abs(delta) > std::abs(diff)) return target;
-        return current + delta;
+      }
     };
 
-    while (g_running) {
-        SDL_Event event;
-        while (SDL_PollEvent(&event)) {
-            if (event.type == SDL_JOYBUTTONDOWN) {
-                if (event.jbutton.button == 0) { // A 버튼 누르면 거리 0m 리셋
-                    trip_initialized = false;
-                } else if (event.jbutton.button == 1 || event.jbutton.button == 6) { // B or Back
-                    g_running = 0;
-                }
-            }
-        }
-
-        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<float>(ly_raw) / 32767.0f);
-        float lx = applyDeadzone(static_cast<float>(lx_raw) / 32767.0f);
-        float rx = applyDeadzone(static_cast<float>(rx_raw) / 32767.0f);
-
-        // 직진 제어 락 (Straight-Drive Lock): 스틱 좌우 15% 이내 기울임은 100% 칼직진 고정!
-        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
-
-        if (std::abs(rx) > 0.0f) {
-            // 제자리 회전 (Spin Turn)
-            omega = -rx * (max_spin_v / (effective_w / 2.0f));
-            float v_l = -omega * (effective_w / 2.0f);
-            float v_r = +omega * (effective_w / 2.0f);
-
-            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 {
-            // 직진 및 커브 차동 선회 (Kinematics)
-            omega = -lx * (max_v / (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;
-        }
-
-        if (state == "STOPPED") {
-            if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
-                driver_front.setBrakes(false);
-                if (driver_rear_ptr) driver_rear_ptr->setBrakes(false);
-                std::this_thread::sleep_for(std::chrono::milliseconds(50));
-                state = "RUNNING";
-            }
-        } else if (state == "RUNNING") {
-            cmd_fl = sCurveStep(cmd_fl, target_fl, max_accel_step, max_decel_step);
-            cmd_fr = sCurveStep(cmd_fr, target_fr, max_accel_step, max_decel_step);
-            cmd_rl = sCurveStep(cmd_rl, target_rl, max_accel_step, max_decel_step);
-            cmd_rr = sCurveStep(cmd_rr, target_rr, max_accel_step, max_decel_step);
-
-            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) {
-                state = "STOPPING";
-            }
-        } else if (state == "STOPPING") {
-            // 감속 시에는 즉각적으로 정지되도록 빠르게 감속 적용 (슬라이딩 감속 지연 제거)
-            cmd_fl = sCurveStep(cmd_fl, target_fl, max_decel_step, max_decel_step);
-            cmd_fr = sCurveStep(cmd_fr, target_fr, max_decel_step, max_decel_step);
-            cmd_rl = sCurveStep(cmd_rl, target_rl, max_decel_step, max_decel_step);
-            cmd_rr = sCurveStep(cmd_rr, target_rr, max_decel_step, max_decel_step);
-
-            if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) {
-                cmd_fl = 0.0f; cmd_fr = 0.0f;
-                cmd_rl = 0.0f; cmd_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";
-            }
-        }
-
-        auto comm_start = std::chrono::high_resolution_clock::now();
-
-        if (state != "STOPPED") {
-            driver_front.setRPMs(cmd_fl, cmd_fr);
-            if (driver_rear_ptr) driver_rear_ptr->setRPMs(cmd_rl, cmd_rr);
-        }
-
-        float fl_fb = 0, fr_fb = 0, rl_fb = 0, rr_fb = 0;
-        int32_t fl_tick = 0, fr_tick = 0, rl_tick = 0, rr_tick = 0;
-
-        driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick);
-        if (driver_rear_ptr) driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick);
-
-        auto comm_end = std::chrono::high_resolution_clock::now();
-        float comm_ms = std::chrono::duration<float, std::milli>(comm_end - comm_start).count();
-
-        // 16384 Ticks = 4배 배율 4채널 엔코더 1회전 (0.798m) 정밀 반영
-        float meters_per_tick = (2.0f * PI_VAL * wheel_radius) / 16384.0f;
-        if (!trip_initialized && (fl_tick != 0 || fr_tick != 0)) {
-            start_fl = fl_tick; start_fr = fr_tick;
-            start_rl = rl_tick; start_rr = rr_tick;
-            trip_initialized = true;
-        }
-
-        int32_t delta_fl = std::abs(fl_tick - start_fl);
-        int32_t delta_fr = std::abs(fr_tick - start_fr);
-        int32_t delta_rl = std::abs(rl_tick - start_rl);
-        int32_t delta_rr = std::abs(rr_tick - start_rr);
-
-        float dist_fl = delta_fl * meters_per_tick;
-        float dist_fr = delta_fr * meters_per_tick;
-        float dist_rl = delta_rl * meters_per_tick;
-        float dist_rr = delta_rr * meters_per_tick;
-        float dist_axle = (dist_fl + dist_fr + dist_rl + dist_rr) / 4.0f;
-        float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);
-
-
-        printf("\r\033[K[%-8s] AXLE: %5.3fm | BUMPER: %5.3fm | Ticks: FL=%+5d FR=%+5d | LATENCY: %4.1fms",
-               state.c_str(), dist_axle, dist_bumper, delta_fl, delta_fr, comm_ms);
-        fflush(stdout);
-
-        std::this_thread::sleep_for(std::chrono::microseconds(static_cast<int>(dt * 1000000)));
+    if (packet->data_type == kLivoxLidarCartesianCoordinateHighData) {
+      LivoxLidarCartesianHighRawPoint *pts =
+          (LivoxLidarCartesianHighRawPoint *)packet->data;
+      for (uint32_t i = 0; i < packet->dot_num; ++i) {
+        parsePoint(pts[i].x / 1000.0f, pts[i].y / 1000.0f, pts[i].z / 1000.0f);
+      }
+    } else if (packet->data_type == kLivoxLidarCartesianCoordinateLowData) {
+      LivoxLidarCartesianLowRawPoint *pts =
+          (LivoxLidarCartesianLowRawPoint *)packet->data;
+      for (uint32_t i = 0; i < packet->dot_num; ++i) {
+        parsePoint(pts[i].x / 100.0f, pts[i].y / 100.0f, pts[i].z / 100.0f);
+      }
     }
 
-    std::cout << "\n\n[정보] C++ 4WD 안전 정지 및 브레이크 잠금 처리 중...\n";
-    driver_front.setRPMs(0, 0);
-    if (driver_rear_ptr) driver_rear_ptr->setRPMs(0, 0);
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    std::lock_guard<std::mutex> lock(status_mutex_);
+    auto updateEMA = [](float &old_val, float new_val) {
+      if (new_val < old_val) {
+        old_val = new_val; // Fast response to approaching obstacles
+      } else {
+        old_val = old_val * 0.70f + new_val * 0.30f; // Smooth recovery
+      }
+    };
 
-    driver_front.setBrakes(true);
-    if (driver_rear_ptr) driver_rear_ptr->setBrakes(true);
+    updateEMA(status_.min_dist_front, curr_front);
+    updateEMA(status_.min_dist_rear, curr_rear);
+    updateEMA(status_.min_dist_left, curr_left);
+    updateEMA(status_.min_dist_right, curr_right);
+    status_.connected = true;
+    status_.last_update = std::chrono::steady_clock::now();
+  }
 
-    if (joystick) SDL_JoystickClose(joystick);
-    SDL_Quit();
+  ObstacleStatus getStatus() {
+    std::lock_guard<std::mutex> lock(status_mutex_);
+    auto now = std::chrono::steady_clock::now();
+    if (std::chrono::duration_cast<std::chrono::milliseconds>(
+            now - status_.last_update)
+            .count() > 800) {
+      status_.connected = false;
+    }
+    return status_;
+  }
+};
 
-    std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
-    return 0;
+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;
 }
+
+int main(int argc, char **argv) {
+  std::signal(SIGINT, signalHandler);
+  std::signal(SIGTERM, signalHandler);
+
+  std::string port1 = "/dev/ttyUSB0";
+  std::string port2 = "";
+  uint8_t id1 = 1;
+  uint8_t id2 = 2;
+  bool bcast_mode = false;
+  float max_v = 0.40f;            // m/s
+  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 accel_rate = 120.0f;      // RPM/s (최대 가속도 한계)
+  float decel_rate = 180.0f;      // RPM/s (최대 감속도 한계)
+  float jerk_rate = 600.0f;       // RPM/s^2 (직진/커브 선회 저크 한계)
+  float spin_jerk_rate = 220.0f;  // RPM/s^2 (제자리 회전 저크 한계: 고마찰 노면에서
+                                   // 정지마찰이 급격히 파괴되지 않도록 accel_rate에
+                                   // 도달하는 시간을 더 길게 늘림)
+  float spin_arc_bias = 0.12f;    // 0~1 (제자리 회전 시 섞을 최소 회전반경용
+                                   // 미세 전진 성분 비율. ICR을 로봇 중심에서
+                                   // 살짝 벗어나게 해 스크럽 마찰 저항을 줄임)
+  float bumper_offset = 0.045f;   // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리
+
+  // 바퀴별 이상(들뜸/걸림) 판정 파라미터: 속도 폐루프 특성상 "지령 대비 실제
+  // 속도" 하나만으로는 무부하(들뜸) 판정이 안 되므로, 동료 바퀴 대비 전류
+  // 편차와 함께 두 축으로 판정한다.
+  float min_active_rpm = 3.0f;        // RPM (이 미만이면 판정 보류: 정지 취급)
+  float airborne_current_ratio = 0.35f; // 동료 바퀴 중앙값 전류 대비 이 비율
+                                         // 미만이면 무부하(들뜸)로 판정
+  float stall_vel_ratio = 0.5f;       // 실제속도/지령속도가 이 비율 미만이면
+                                       // 속도 지연으로 판단(걸림 후보)
+  float stall_current_a = 12.0f;      // A (정격 15A 근접) 이상이면서 속도
+                                       // 지연이 동반되면 걸림/과부하로 판정
+  int airborne_debounce_ticks = 5;    // 틱 (100Hz 기준 50ms) 연속 들뜸 판정
+                                       // 시에만 실제로 목표속도를 낮춤
+  std::string log_path = ""; // 비어있으면 로깅 비활성화. --log <파일경로>로 지정
+
+  // LiDAR Parameters & Robot Physical Specs (User Specification)
+  bool use_lidar = true;
+  std::string lidar_config = "mid360s_config.json";
+  float front_stop_dist = 0.35f; // m (전방 완전 정지 거리 0.35m)
+  float front_warn_dist = 0.50f; // m (전방 감속/경고 시작 거리 0.50m = 50cm)
+  float side_dodge_dist = 0.50f; // m (측면 장애물 우회 감지 거리 0.50m = 50cm)
+  float side_stop_dist = 0.20f;  // m (측면 한계 접근 거리 0.20m)
+  float max_dodge_omega = 0.35f; // rad/s (자동 회피 최대 조향 각속도)
+  float robot_width = 0.410f;    // m (로봇 가로/좌우 실측 전폭 410mm)
+  float robot_length = 0.631f;   // m (로봇 세로/전후 실측 전장 631mm)
+  float robot_height = 0.286f;   // m (로봇 전고 286mm)
+  float lidar_height = 0.460f;   // m (지면 기준 라이다 높이 460mm)
+  float lidar_pitch = 17.0f;     // deg (라이다 하향 기울임 17도)
+  float lidar_x_offset = 0.250f; // m (로봇 중심 기준 라이다 전방 오프셋 250mm)
+  float min_z = -lidar_height + 0.06f; // m (-0.40m: 지면 감지 방지 안전 오프셋)
+  float max_z = 0.80f;                 // m (라이다 기준 상단 높이)
+
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg == "--port" && i + 1 < argc)
+      port1 = argv[++i];
+    else if (arg == "--port1" && i + 1 < argc)
+      port1 = argv[++i];
+    else if (arg == "--port2" && i + 1 < argc)
+      port2 = argv[++i];
+    else if (arg == "--id1" && i + 1 < argc)
+      id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
+    else if (arg == "--id2" && i + 1 < argc)
+      id2 = static_cast<uint8_t>(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 == "--radius" && i + 1 < argc)
+      wheel_radius = std::stof(argv[++i]);
+    else if (arg == "--bumper" && i + 1 < argc)
+      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)
+      decel_rate = std::stof(argv[++i]);
+    else if (arg == "--jerk" && i + 1 < argc)
+      jerk_rate = std::stof(argv[++i]);
+    else if (arg == "--spin_jerk" && i + 1 < argc)
+      spin_jerk_rate = std::stof(argv[++i]);
+    else if (arg == "--spin_arc_bias" && i + 1 < argc)
+      spin_arc_bias = std::stof(argv[++i]);
+    else if (arg == "--min_active_rpm" && i + 1 < argc)
+      min_active_rpm = std::stof(argv[++i]);
+    else if (arg == "--airborne_current_ratio" && i + 1 < argc)
+      airborne_current_ratio = std::stof(argv[++i]);
+    else if (arg == "--stall_vel_ratio" && i + 1 < argc)
+      stall_vel_ratio = std::stof(argv[++i]);
+    else if (arg == "--stall_current_a" && i + 1 < argc)
+      stall_current_a = std::stof(argv[++i]);
+    else if (arg == "--airborne_debounce_ticks" && i + 1 < argc)
+      airborne_debounce_ticks = std::stoi(argv[++i]);
+    else if (arg == "--log" && i + 1 < argc)
+      log_path = argv[++i];
+    else if (arg == "--bcast" || arg == "--broadcast")
+      bcast_mode = true;
+    else if (arg == "--lidar_config" && i + 1 < argc)
+      lidar_config = argv[++i];
+    else if (arg == "--front_stop" && i + 1 < argc)
+      front_stop_dist = std::stof(argv[++i]);
+    else if (arg == "--front_warn" && i + 1 < argc)
+      front_warn_dist = std::stof(argv[++i]);
+    else if (arg == "--side_dodge" && i + 1 < argc)
+      side_dodge_dist = std::stof(argv[++i]);
+    else if (arg == "--robot_width" && i + 1 < argc)
+      robot_width = std::stof(argv[++i]);
+    else if (arg == "--lidar_height" && i + 1 < argc)
+      lidar_height = std::stof(argv[++i]);
+    else if (arg == "--lidar_pitch" && i + 1 < argc)
+      lidar_pitch = std::stof(argv[++i]);
+    else if (arg == "--lidar_x_offset" && i + 1 < argc)
+      lidar_x_offset = std::stof(argv[++i]);
+    else if (arg == "--min_z" && i + 1 < argc)
+      min_z = std::stof(argv[++i]);
+    else if (arg == "--max_z" && i + 1 < argc)
+      max_z = std::stof(argv[++i]);
+    else if (arg == "--no_lidar")
+      use_lidar = false;
+  }
+
+  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;
+
+  std::cout << "\n============================================================="
+               "=====\n";
+  std::cout
+      << " [C++17 4WD ZLAC8015D + Mid-360S 라이다 정밀 장애물 회피 시스템]\n";
+  std::cout << "  - 로봇 스펙: 가로(전폭) " << robot_width * 1000.0f
+            << "mm | 세로(전장) " << robot_length * 1000.0f << "mm | 높이 "
+            << robot_height * 1000.0f << "mm\n";
+  std::cout << "  - 라이다 설치: 지면 높이 " << lidar_height * 1000.0f
+            << "mm | 피치 경사각 " << lidar_pitch << "도 (하향) | 전방 오프셋 "
+            << lidar_x_offset * 1000.0f << "mm\n";
+  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 << "  - 휠 이상판정: 무부하(들뜸) 전류비 " << airborne_current_ratio
+            << " | 걸림 속도비 " << stall_vel_ratio << " / 전류 "
+            << stall_current_a << "A 이상\n";
+  std::cout << "  - 라이다 장애물 회피 (LiDAR Dodge): "
+            << (use_lidar ? "활성화 (Mid-360S)" : "비활성화") << "\n";
+  std::cout << "  - 전방 정지 거리: " << front_stop_dist
+            << "m | 전방 경고: " << front_warn_dist
+            << "m | 측면 회피: " << side_dodge_dist << "m\n";
+  std::cout
+      << "==================================================================\n";
+
+  // CSV 로깅: --log <경로> 지정 시 매 틱 지령/피드백/전류/휠상태를 파일로 기록.
+  // 실외에서 로봇을 조종하며 화면을 동시에 읽기 어려우므로, 문제가 된
+  // 구간(턱 넘는 지점 등)을 나중에 잘라서 분석하기 위한 용도.
+  std::ofstream log_stream;
+  if (!log_path.empty()) {
+    log_stream.open(log_path);
+    if (log_stream.is_open()) {
+      log_stream << "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,"
+                    "dist_axle,comm_ms\n";
+      std::cout << "  - CSV 로그 기록: " << log_path << "\n";
+    } else {
+      std::cerr << "[경고] 로그 파일 열기 실패: " << log_path << "\n";
+    }
+  }
+  auto log_start_time = std::chrono::steady_clock::now();
+
+  // Initialize LiDAR Detector
+  LidarObstacleDetector lidar_detector;
+  if (use_lidar) {
+    // Robot half width tolerance = robot_width / 2 + 5cm margin
+    lidar_detector.setParams(min_z, max_z, robot_width / 2.0f + 0.05f,
+                             lidar_pitch, lidar_height, lidar_x_offset);
+    if (!lidar_detector.init(lidar_config)) {
+      std::cout << "[경고] 라이다 연결 실패. 장애물 감지 없이 조이스틱 모드로 "
+                   "실행합니다.\n";
+    }
+  }
+
+  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;
+
+  SerialPort sp1, sp2;
+  if (!sp1.openPort(port1))
+    return 1;
+  std::cout << "[정보] RS485 포트1 (" << port1 << ") 오픈 성공.\n";
+
+  SerialPort *sp2_ptr = &sp1;
+  if (!port2.empty()) {
+    if (sp2.openPort(port2)) {
+      sp2_ptr = &sp2;
+      std::cout << "[정보] RS485 포트2 (" << port2 << ") 오픈 성공.\n";
+    }
+  }
+
+  MotorDriver driver_front(&sp1, bcast_mode ? 0 : id1);
+  driver_front.initDriver(150, 150);
+
+  MotorDriver *driver_rear_ptr = nullptr;
+  MotorDriver driver_rear(sp2_ptr, bcast_mode ? 0 : id2);
+  if (!bcast_mode) {
+    driver_rear.initDriver(150, 150);
+    driver_rear_ptr = &driver_rear;
+  }
+
+  std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 "
+               "또는 Ctrl+C, 라이다 토글: X 버튼)\n\n";
+
+  float target_fl = 0.0f, target_fr = 0.0f;
+  float target_rl = 0.0f, target_rr = 0.0f;
+
+  float cmd_fl = 0.0f, cmd_fr = 0.0f;
+  float cmd_rl = 0.0f, cmd_rr = 0.0f;
+  // 각 바퀴의 현재 가속도 상태(저크 제한 프로파일용)
+  float accel_fl = 0.0f, accel_fr = 0.0f;
+  float accel_rl = 0.0f, accel_rr = 0.0f;
+
+  const float loop_hz = 100.0f;
+  const float dt = 1.0f / loop_hz;
+
+  // 저크 제한(jerk-limited) S-curve: 가속도 자체를 매 틱 jerk_limit 만큼만
+  // 변화시켜 서서히 목표 가속도에 도달하게 함. 기존 구현은 목표 접근 구간만
+  // 부드러웠고 정지 상태에서 출발할 때는 첫 틱부터 거의 풀가속도가 걸려
+  // 정지마찰이 급격히 파괴되는 문제가 있었음.
+  auto jerkLimitedStep = [dt](float current_vel, float target_vel,
+                              float &current_accel, float max_accel_mag,
+                              float max_decel_mag, float jerk_limit) {
+    float diff = target_vel - current_vel;
+    if (std::abs(diff) < 0.01f) {
+      current_accel = 0.0f;
+      return target_vel;
+    }
+
+    bool is_accelerating = std::abs(target_vel) > std::abs(current_vel);
+    float accel_cap = is_accelerating ? max_accel_mag : max_decel_mag;
+    // 이번 틱에 diff를 없애는데 필요한 가속도(부호 포함)를 물리적 한계로 클램프
+    float desired_accel = std::clamp(diff / dt, -accel_cap, accel_cap);
+
+    float max_delta_accel = jerk_limit * dt;
+    current_accel = std::clamp(desired_accel, current_accel - max_delta_accel,
+                               current_accel + max_delta_accel);
+
+    float next_vel = current_vel + current_accel * dt;
+    bool overshoot =
+        (diff > 0.0f) ? (next_vel >= target_vel) : (next_vel <= target_vel);
+    if (overshoot) {
+      current_accel = 0.0f;
+      return target_vel;
+    }
+    return next_vel;
+  };
+
+  std::string state = "STOPPED";
+  bool trip_initialized = false;
+  int32_t start_fl = 0, start_fr = 0, start_rl = 0, start_rr = 0;
+
+  // 바퀴별 이상 상태(직전 틱 피드백 기반 판정, 1틱=10ms 지연으로 반영)
+  bool airborne_fl = false, airborne_fr = 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;
+  char wheel_status_str[5] = "OOOO";
+
+  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;
+        }
+      }
+    }
+
+    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<float>(ly_raw) / 32767.0f);
+    float lx = applyDeadzone(static_cast<float>(lx_raw) / 32767.0f);
+    float rx = applyDeadzone(static_cast<float>(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
+    // (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작)
+    // ---------------------------------------------------------------------
+    std::string lidar_telemetry = "LIDAR:OFF";
+    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) {
+          if (obs.min_dist_front < front_stop_dist) {
+            speed_scale = 0.0f; // Complete forward stop
+          } else if (obs.min_dist_front < front_warn_dist) {
+            speed_scale = (obs.min_dist_front - front_stop_dist) /
+                          (front_warn_dist - front_stop_dist);
+            speed_scale = std::clamp(speed_scale, 0.0f, 1.0f);
+          }
+        } else if (v_x < -0.01f) {
+          if (obs.min_dist_rear < front_stop_dist) {
+            speed_scale = 0.0f; // Complete backward stop
+          }
+        }
+
+        // 2. Side Dodge Steering (전진 중에만 동작)
+        if (v_x > 0.01f && rx == 0.0f) {
+          float right_intensity = 0.0f;
+          float left_intensity = 0.0f;
+
+          if (obs.min_dist_right < side_dodge_dist) {
+            right_intensity = (side_dodge_dist - obs.min_dist_right) /
+                              (side_dodge_dist - side_stop_dist);
+            right_intensity = std::clamp(right_intensity, 0.0f, 1.0f);
+          }
+          if (obs.min_dist_left < side_dodge_dist) {
+            left_intensity = (side_dodge_dist - obs.min_dist_left) /
+                             (side_dodge_dist - side_stop_dist);
+            left_intensity = std::clamp(left_intensity, 0.0f, 1.0f);
+          }
+
+          // Right obstacle -> Steer Left (+omega)
+          // Left obstacle  -> Steer Right (-omega)
+          avoid_omega_offset =
+              (right_intensity - left_intensity) * max_dodge_omega;
+        }
+
+        v_x *= speed_scale;
+
+        char lbuf[128];
+        snprintf(lbuf, sizeof(lbuf), "F:%4.2fm|L:%4.2fm|R:%4.2fm %s",
+                 obs.min_dist_front > 99.0f ? 9.99f : obs.min_dist_front,
+                 obs.min_dist_left > 99.0f ? 9.99f : obs.min_dist_left,
+                 obs.min_dist_right > 99.0f ? 9.99f : obs.min_dist_right,
+                 avoid_omega_offset > 0.05f
+                     ? "[Dodge L]"
+                     : (avoid_omega_offset < -0.05f
+                            ? "[Dodge R]"
+                            : (speed_scale < 0.01f ? "[STOP!]" : "[OK]")));
+        lidar_telemetry = lbuf;
+
+        // Add avoidance offset to user steering
+        omega += avoid_omega_offset;
+      } else {
+        lidar_telemetry = "LIDAR:WAITING";
+      }
+    }
+
+    // 제자리 회전 여부: 왼쪽 스틱 전후(ly) 입력이 완전히 중립일 때만 진입한다.
+    // 그렇지 않으면(주행 중 오른쪽 스틱이 드리프트 등으로 미세하게라도 감지될
+    // 경우) 전진 입력이 통째로 무시되고 제자리 회전 로직으로 튀어버려, 커브
+    // 선회 중에 갑자기 이상하게 회전하려는 문제가 생긴다. ly는 applyDeadzone을
+    // 거쳐 데드존 이하일 때 정확히 0.0f를 반환하므로 등호 비교가 안전하다.
+    bool is_spin_turn = std::abs(rx) > 0.0f && ly == 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;
+      float v_l = v_bias - omega * (effective_w / 2.0f);
+      float v_r = v_bias + omega * (effective_w / 2.0f);
+
+      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 {
+      // 직진 및 차동 선회 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 += -lx * (max_v / (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으로 낮춰
+    // 헛돌이를 억제한다. 접지력이 회복되어 전류가 정상으로 돌아오면 다음
+    // 판정 틱에서 자동으로 해제되어 원래 지령으로 복귀한다.
+    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;
+
+    if (state == "STOPPED") {
+      if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
+        driver_front.setBrakes(false);
+        if (driver_rear_ptr)
+          driver_rear_ptr->setBrakes(false);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+        state = "RUNNING";
+      }
+    } else if (state == "RUNNING") {
+      float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate;
+      cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, accel_rate,
+                               decel_rate, jerk_now);
+      cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, accel_rate,
+                               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 &&
+          std::abs(cmd_fl) < 0.5f && std::abs(cmd_fr) < 0.5f) {
+        state = "STOPPING";
+      }
+    } else if (state == "STOPPING") {
+      float jerk_now = is_spin_turn ? spin_jerk_rate : jerk_rate;
+      cmd_fl = jerkLimitedStep(cmd_fl, target_fl, accel_fl, decel_rate,
+                               decel_rate, jerk_now);
+      cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate,
+                               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) {
+        cmd_fl = 0.0f;
+        cmd_fr = 0.0f;
+        cmd_rl = 0.0f;
+        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";
+      }
+    }
+
+    auto comm_start = std::chrono::high_resolution_clock::now();
+
+    if (state != "STOPPED") {
+      driver_front.setRPMs(cmd_fl, cmd_fr);
+      if (driver_rear_ptr)
+        driver_rear_ptr->setRPMs(cmd_rl, cmd_rr);
+    }
+
+    float fl_fb = 0, fr_fb = 0, rl_fb = 0, rr_fb = 0;
+    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;
+
+    driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick, fl_amp, fr_amp);
+    if (driver_rear_ptr)
+      driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick, rl_amp,
+                                    rr_amp);
+
+    auto comm_end = std::chrono::high_resolution_clock::now();
+    float comm_ms =
+        std::chrono::duration<float, std::milli>(comm_end - comm_start).count();
+
+    // 바퀴별 이상(들뜸/걸림) 판정: 속도 폐루프 특성상 "지령 대비 실제속도"
+    // 만으로는 무부하(들뜸)를 구분할 수 없다 (드라이버가 부하와 무관하게
+    // 지령 RPM을 그대로 추종하려 하므로). 그래서 두 축으로 판정한다.
+    //  - 걸림/과부하: 실제속도가 지령을 크게 못 따라가면서 전류가 높음 (독립 판정)
+    //  - 무부하/들뜸: "같은 지령속도를 받는 짝(앞뒤 같은 쪽)" 대비 전류가
+    //    비정상적으로 낮음. 좌/우를 통째로 비교하면 정상적인 커브 선회에서
+    //    지령속도가 원래 다른 좌우 바퀴를 오탐하므로, 반드시 target_fl=target_rl,
+    //    target_fr=target_rr 로 지령이 항상 같은 같은쪽 앞뒤 쌍끼리만 비교한다.
+    auto classifyStall = [&](float cmd_rpm, float actual_rpm,
+                             float amp) -> bool {
+      float cmd_abs = std::abs(cmd_rpm);
+      if (cmd_abs < min_active_rpm)
+        return false;
+      float vel_ratio = std::abs(actual_rpm) / cmd_abs;
+      return vel_ratio < stall_vel_ratio && std::abs(amp) >= stall_current_a;
+    };
+    bool stall_fl = classifyStall(cmd_fl, fl_fb, fl_amp);
+    bool stall_fr = classifyStall(cmd_fr, fr_fb, fr_amp);
+    bool stall_rl = classifyStall(cmd_rl, rl_fb, rl_amp);
+    bool stall_rr = classifyStall(cmd_rr, rr_fb, rr_amp);
+
+    bool air_now_fl = false, air_now_rl = false;
+    bool air_now_fr = false, air_now_rr = false;
+    auto classifyAirPair = [&](float cmd_rpm, float amp_a, float amp_b,
+                               bool &air_a, bool &air_b) {
+      if (std::abs(cmd_rpm) < min_active_rpm)
+        return; // 정지/저속 중이면 판정 보류
+      float a = std::abs(amp_a), b = std::abs(amp_b);
+      float hi = std::max(a, b);
+      if (hi < 0.5f)
+        return; // 둘 다 거의 무전류(관성 주행 등)면 판정 보류
+      if (a < hi * airborne_current_ratio)
+        air_a = true;
+      if (b < hi * airborne_current_ratio)
+        air_b = true;
+    };
+    classifyAirPair(cmd_fl, fl_amp, rl_amp, air_now_fl, air_now_rl);
+    classifyAirPair(cmd_fr, fr_amp, rr_amp, air_now_fr, air_now_rr);
+
+    // 디바운스: 노이즈성 순간 전류 편차로 목표속도가 즉시 0으로 깎이지 않도록
+    // 연속 airborne_debounce_ticks 틱 이상 지속될 때만 실제로 개입한다.
+    air_count_fl = air_now_fl ? air_count_fl + 1 : 0;
+    air_count_fr = air_now_fr ? air_count_fr + 1 : 0;
+    air_count_rl = air_now_rl ? air_count_rl + 1 : 0;
+    air_count_rr = air_now_rr ? air_count_rr + 1 : 0;
+
+    airborne_fl = air_count_fl >= airborne_debounce_ticks;
+    airborne_fr = air_count_fr >= airborne_debounce_ticks;
+    airborne_rl = air_count_rl >= airborne_debounce_ticks;
+    airborne_rr = air_count_rr >= airborne_debounce_ticks;
+
+    auto statusChar = [](bool air, bool stall) {
+      return air ? 'A' : (stall ? 'S' : 'O');
+    };
+    wheel_status_str[0] = statusChar(airborne_fl, stall_fl);
+    wheel_status_str[1] = statusChar(airborne_fr, stall_fr);
+    wheel_status_str[2] = statusChar(airborne_rl, stall_rl);
+    wheel_status_str[3] = statusChar(airborne_rr, stall_rr);
+
+    float meters_per_tick = (2.0f * PI_VAL * wheel_radius) / 16384.0f;
+    if (!trip_initialized && (fl_tick != 0 || fr_tick != 0)) {
+      start_fl = fl_tick;
+      start_fr = fr_tick;
+      start_rl = rl_tick;
+      start_rr = rr_tick;
+      trip_initialized = true;
+    }
+
+    int32_t delta_fl = std::abs(fl_tick - start_fl);
+    int32_t delta_fr = std::abs(fr_tick - start_fr);
+    int32_t delta_rl = std::abs(rl_tick - start_rl);
+    int32_t delta_rr = std::abs(rr_tick - start_rr);
+
+    float dist_fl = delta_fl * meters_per_tick;
+    float dist_fr = delta_fr * meters_per_tick;
+    float dist_rl = delta_rl * meters_per_tick;
+    float dist_rr = delta_rr * meters_per_tick;
+
+    // 중앙값(4개 중 가운데 2개 평균) 기반 주행거리 산출: 웅덩이 등으로 바퀴
+    // 하나가 지면에서 떨어져 엔코더만 헛돌 때, 그 이상치 값이 평균(mean)을
+    // 오염시키지 않도록 최댓값/최솟값을 자동으로 배제함.
+    float dist_sorted[4] = {dist_fl, dist_fr, dist_rl, dist_rr};
+    std::sort(dist_sorted, dist_sorted + 4);
+    float dist_axle = (dist_sorted[1] + dist_sorted[2]) / 2.0f;
+    float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);
+    (void)dist_bumper;
+
+    if (log_stream.is_open()) {
+      float t_ms = std::chrono::duration<float, std::milli>(
+                       std::chrono::steady_clock::now() - log_start_time)
+                       .count();
+      log_stream << t_ms << ',' << state << ',' << ly << ',' << lx << ','
+                << rx << ',' << 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]
+                << ',' << wheel_status_str[1] << ',' << wheel_status_str[2]
+                << ',' << wheel_status_str[3] << ',' << dist_axle << ','
+                << comm_ms << '\n';
+    }
+
+    printf("\r\033[K[%-8s] AXLE:%5.3fm | %-28s | I(A):FL%+4.1f FR%+4.1f "
+          "RL%+4.1f RR%+4.1f | WHL(FL/FR/RL/RR):%s | %4.1fms",
+          state.c_str(), dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
+          rl_amp, rr_amp, wheel_status_str, comm_ms);
+    fflush(stdout);
+
+    std::this_thread::sleep_for(
+        std::chrono::microseconds(static_cast<int>(dt * 1000000)));
+  }
+
+  std::cout << "\n\n[정보] C++ 4WD 안전 정지 및 브레이크 잠금 처리 중...\n";
+  driver_front.setRPMs(0, 0);
+  if (driver_rear_ptr)
+    driver_rear_ptr->setRPMs(0, 0);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+  driver_front.setBrakes(true);
+  if (driver_rear_ptr)
+    driver_rear_ptr->setBrakes(true);
+
+  if (use_lidar)
+    lidar_detector.stop();
+
+  if (log_stream.is_open()) {
+    log_stream.close();
+    std::cout << "[정보] CSV 로그 저장 완료: " << log_path << "\n";
+  }
+
+  if (joystick)
+    SDL_JoystickClose(joystick);
+  SDL_Quit();
+
+  std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
+  return 0;
+}
\ No newline at end of file
diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp
index d9af75a..3df3056 100755
Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ