From 9f2ead21384d2af23399e2cecb0750a35d46275b Mon Sep 17 00:00:00 2001 From: robin Date: Wed, 12 Aug 2026 16:31:06 +0900 Subject: [PATCH] Improve outdoor traction control and raise top speed Adds jerk-limited S-curve motion profiles, robust median-based odometry to reject wheel-lift encoder spikes, per-wheel current-based airborne/stall detection with pair-wise comparison to avoid false positives during normal curve turns, curved-turn blending for spin turns, and optional CSV telemetry logging. Fixes a bug where any right-stick input while driving forward would override left-stick curve steering. Raises max speed from 0.3 to 0.4 m/s. Co-Authored-By: Claude Sonnet 5 --- Makefile | 6 +- mid360s_config.json | 21 + run_4wd.sh | 16 +- xbox_motor_control.cpp | 1611 ++++++++++++++++++++++++++++------------ xbox_motor_control_cpp | Bin 41904 -> 60104 bytes 5 files changed, 1170 insertions(+), 484 deletions(-) create mode 100644 mid360s_config.json 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 -#include -#include -#include -#include -#include +#include #include +#include +#include +#include #include #include -#include -#include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include + +#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(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(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(rx_buf[6]) | + (static_cast(rx_buf[7]) << 8); + return resp_crc == recv_crc && rx_buf[0] == slave; + } + + bool writeRegs(uint8_t slave, uint16_t reg, + const std::vector &vals) { + size_t count = vals.size(); + size_t pkt_len = 7 + count * 2 + 2; + std::vector 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(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& vals) { - size_t count = vals.size(); - size_t pkt_len = 7 + count * 2 + 2; - std::vector 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(count * 2); + tcflush(fd_, TCIOFLUSH); + ssize_t written = write(fd_, pkt.data(), pkt_len); + if (written != static_cast(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(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(pkt_len)) return false; + uint16_t resp_crc = calcCRC16(rx_buf, 6); + uint16_t recv_crc = static_cast(rx_buf[6]) | + (static_cast(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 &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(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 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(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& 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 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(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(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(rx_buf[expected_bytes - 2]) | + (static_cast(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(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(std::clamp(l_rpm, -3000.0f, 3000.0f)); - int16_t r_val = static_cast(std::clamp(r_rpm, -3000.0f, 3000.0f)); - port_->writeRegs(slave_id_, 0x2088, {static_cast(l_val), static_cast(r_val)}); - } - - bool readFeedback(float& l_fb, float& r_fb, int32_t& l_tick, int32_t& r_tick) { - std::vector regs; - if (port_->readRegs(slave_id_, 0x20A7, 6, regs)) { - uint32_t val_l = ((static_cast(regs[0]) & 0xFFFF) << 16) | (regs[1] & 0xFFFF); - l_tick = (val_l < 0x80000000U) ? static_cast(val_l) : static_cast(val_l - 0x100000000ULL); - - uint32_t val_r = ((static_cast(regs[2]) & 0xFFFF) << 16) | (regs[3] & 0xFFFF); - r_tick = (val_r < 0x80000000U) ? static_cast(val_r) : static_cast(val_r - 0x100000000ULL); - - int16_t vl = static_cast(regs[4]); - int16_t vr = static_cast(regs[5]); - l_fb = static_cast(vl); - r_fb = static_cast(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(std::clamp(l_rpm, -3000.0f, 3000.0f)); + int16_t r_val = static_cast(std::clamp(r_rpm, -3000.0f, 3000.0f)); + port_->writeRegs( + slave_id_, 0x2088, + {static_cast(l_val), static_cast(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 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(regs[0]) << 16) | regs[1]; + l_tick = static_cast(val_l); + + uint32_t val_r = (static_cast(regs[2]) << 16) | regs[3]; + r_tick = static_cast(val_r); + + int16_t vl = static_cast(regs[4]); + int16_t vr = static_cast(regs[5]); + l_fb = static_cast(vl) * 0.1f; // 0.1RPM ๋‹จ์œ„ -> RPM + r_fb = static_cast(vr) * 0.1f; + + int16_t tl = static_cast(regs[6]); + int16_t tr = static_cast(regs[7]); + l_torque_a = static_cast(tl) * 0.1f; // 0.1A ๋‹จ์œ„ -> A + r_torque_a = static_cast(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 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(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 == "--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(ly_raw) / 32767.0f); - float lx = applyDeadzone(static_cast(lx_raw) / 32767.0f); - float rx = applyDeadzone(static_cast(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(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(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 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 lock(status_mutex_); + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast( + 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(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 == "--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 ¤t_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(ly_raw) / 32767.0f); + float lx = applyDeadzone(static_cast(lx_raw) / 32767.0f); + float rx = applyDeadzone(static_cast(rx_raw) / 32767.0f); + + if (std::abs(lx) < 0.15f) + lx = 0.0f; + if (std::abs(rx) < 0.15f) + rx = 0.0f; + + float v_x = ly * max_v; // ์„ ์†๋„ m/s + float omega = 0.0f; // ๊ฐ์†๋„ rad/s + + // --------------------------------------------------------------------- + // LiDAR Obstacle Avoidance & Stopping Logic Integration + // (์š”์ฒญ์— ๋”ฐ๋ผ ํ›„์ง„ ์‹œ ์ธก๋ฉด ํšŒํ”ผ๋Š” ๋ฏธ์ ์šฉ - ์ „์ง„ ์‹œ์—๋งŒ ์ขŒ์šฐ ํšŒํ”ผ ๋™์ž‘) + // --------------------------------------------------------------------- + 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(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( + 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(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 d9af75a55bb365fdefc2529b215c4623ae1b0042..3df30569fecd0c2b0dc028e75cffc437d4b5c357 100755 GIT binary patch literal 60104 zcmeFa4O~>!_BVb87)4ahP+DYJQ$ZIJ1yl;(5_Hf*jYfj0`GUxclDq{2TA5-n$}~>W z3q>zapPcJHsSlE~2)p}$us+XYD zOHk@*sbbFrB~|T7?C2Y>*wZ(SK^cA2)VJOerJiqU;nf5+B^mTd{nD28@5oD0>dBiq zAK!F5bre2os>-_q^~f&&>PP8zVTK~Fw|+RP2vAc+ybzt2yXdCT(Ro?J^KuJHmk(ba zf79@rMvo{d95IrqK-J9nl21*YK2tz7X7c76VH%WSw8*`L_>O09ipKR@l(25gQp`$G-MAy;T7Rp;~+EP?!Y${--Frj=0^-& zcU5@)fXR2SdU{3fhDnA&wF4%X*sWKhxW^B<+a*1eTn8aN;TQHopA3KQiGC>>(G#B9 zYsT02^Kvh8b7AnF_`K2!{U8{yC;I$e^gQ1SpD%mSKfD({)xGe!2_4syJ`;PPKLNw^ zME^i9^!N6nkD(X*XT8|R)r;ICy~rhhG~?_0iRne|>%GVw)(ic67#uy>A*C1k>wEE& zt9zlJ-ito9z4ZIUUijP&1N5Y4OE3Dz_M+z}z1aD(Ui@caFZ^%rMbEc;;S;F% zi$Az~(I>DM`%LMD|LeWzIS=^jP{;RkIrxVNR|+YIc!?Ts9f92lzfi%OpdWqO7=M#c zeItk8PT;A8^6L;49HfDR&taYjZ=>IdPr^ZtQ2D$CI}m;OYL15*{F9!(IKQwUy~LJL zY)elU(v#DsrDtUqXD`kzv1J#hO`DuoSdg8Tu_!N_m-SeZYO$s#7v$Om%1kfKzqRn* z5?gNO(h`PHA+RdRW|*njwxr_X!eR!b6z1h6m1P(3+IN&HY}8t}7iAZ8V}r_x%X7Q4 zo}Q7PEm(8Q3YS}RvoeZPvzE>*$Yly;+q~$M!rTHIv?$G*oROC|DI;^Kkg6}ARam@q zT45G+)MJwiatbFe$tYNyO|=$LEiX$*VhvBfYx)?NvY<3QbNTX{(qYfSjO4uB%v*Dl zl9JM5(o?3SrKjAM7Mq@uk~%8gQj|6_Jt^h3%+$E_l>F2&u=`!pZ%eh^oKDqaV`E1y z$|%Xr1efC6g2l<1skYeI%q1Dc>9*pGTw6(UW>RVf{v_R;S(smxQJhVzz#`Q)GA2FU zwxqalS$bY}!D8Ez^lUOSFP;8h=RYQWT6#uFN$%o;q#3uRj%I46+Qx9@atooz?=?$` z%goEpC{9Aj$n@mG68hRQY;d2uvhq^O@~Nq*wwp4cWkKPn^gBn!jx2$E)0gFDWfvro zWgszjG*`PY2Ug9_$nR12$+1i}3^`ggWNtxOMqX}KdPec$(tLQSX3TgPARRojN0SFw zc+G#q$gweKW^r~=UPfkiQhq-8GaI2XFdzwlBjn`fQKN3oElDrLcu+M{$ zH`O*e69CJ_!X3F!EXv4A&&(*Xar4?@M|qhqD$VK9AfuT$=mc#>vF@btm`{VGhkmqW zXJp;my&uyTFJGQslwDj>SO7EH?oBU?(X@hNP*13R+>R)pOxG|@r~eNd$Z47sd07xPC>W`QkF$j{Et zFDxT&*oo5&N$XaAMs9&nkWo-rl9!!bB+!JFktf(P7iVMUv0)f6O<<_ySX|&nwnC^( z3FB9i4U?sp6zJ3|v1O4Vb6Bqn%o79}2~-O+D_HXqTP7+q5zs5M=w2Z+2Z-E~qU`J} z@PwTT3aMckw%o!3URbCWLiNn0WQ_EjjNCi{&B5w9a`?y*F?5^|Lx?_Sj1;WIgY>g37kqri2lHF?tH^pPV*j)>Fp zqegqPBfNQUE>_9To(&$ON5qa1CeNOoI4LF9AFFE+`U9f;0DSv_dLZB+RBTpdG6NO(Dob3vt4;BIuAd-whxw!)cw0@8RbACp~kIEls z_5Fpbm4MO!+6ILeiiO;ZPa*;wC`2mt-~ankM2h`|cm;N>ibQ#kFj;}`Vf6ikI}|vB z!GXd;1-|#t-_hOxp-_Qsk&hzhC#=9<`l#oc3MdriaeRz&ZmHrIF6H=mAAF2LAE?B! zD!mFfy~pdpXzaX;LxQkRho`eI^>a{%AD|+U9@XKm(BV($@FR41S%<$#hd-;sC+P4O zb@*vIe5lec3iBg%cr|WE=dd$7wMgAB5dL-*iB#P$5I#kRuRXv4ayMlw!4MmCcyze- z*`&i`E24e2>hRctYM%xjo@A?^CLNw+tDk*3yxNykc2I|h!)u?TI((3Z6@(KyJRDH_ z$U6K$4J!y|b@*T%{-O>q>hQw593b6kT&bTx9X>=wA~oso`h9Gu4u7eR-mJsZxv%<( z(BTKGNTe}3{AD_Pybgc44xgaID~}*psieaX(b1>q@cKjG**d)Xtc9vA)ZvHf_$<-k zl}9bCv`B})Mn}I~hd1l+_={_BPn2!FS4j-k%AJyT9>+mOZ_-Gwo*5PAx__I2EtPX!s zhaaiK3(9;&{y$2G57gmD>+mKWK2C=Z)!}c};mtaHybd3s!;jJ7V|4hjI()niKTd~F z(Ba4H@RANcL5EM#;cwC5XY242b@+uke4-A&M2DZG!x!oBlXdvzI=n@PFW2FdboeSA zeu@rXt;0{%;cInxNr&H{!zb(Tn{@bFb@;71{B1gXgAQ-i;s2BUf5QUj#jAgkDmnut zr*Z$^1wpbm+59{QrHZ|QEv$i_xJ6q8;hZNt6Nz|@K=ezfYV!?4W_{2oI68IbA0s*vNgDC(&`!$#rezaeMX~9SPHJAcgv|od1;Ya&5m==7r zUxR6(NBcFH7Wi}S5Dlh<{hZsN!L*>CbAS7*+I};GKi6Pdz|npUriB~r*I-(((S8l4 zg&OVGU|OKjehsFD8SU3#T9DCx4W@+{?bl!mtkHfArUe=8*I-(R(S8l41sLtuU|M+5 zehnVMV1ounGx*zI)b`WDi~iSOT5!>R4W@+_?bl#hV9|aJriB&l*I-&u(S8l44F}q< z!L+cV{TfUQD%!8Xw2-3x8cYi)+ONU1aH9PhObaI3ufeoXqWv073ncnqgK1$z|NpGE zKY_uYYcMU4Xuk&2!ie^3FfE8^zXsDni1w>6W|s*s)9eB~%_o3=-P9fa-UsgTfxq;D zKlOo+_`nBz;8q{F(Fd;gfw%a;8-3t9AGpQ`cKN_|ANTH{C~fe-k=tv+z0 z4_xm9Z}EXQ`oMKQaE%Y_@`3F>@B=<@sSlj*184id^L^l%KJYXjILQZ|-~-3`z{7pu za36T64?Nfh9_Rxbec)eT^yz;exXTCr(g*%jg(b%stK%E_OUyM^$Jg>uTGJ%wiZHX} zw1ve;k-tfHkIg8zq8z01uQccHqV<@l|4vvCClGOpMqH~99|W=FY>c7AWlx}FOuNk_ zIeFeyWRUY2hsJ75rPeVXR<;3oiOTL!$|!N!H&CKfXL%wS!AXiZNx6=M<%y&}CrRNX z3w0zcPbBhBU=2MvNrH}q<%y(~NF1NZzmjs!#)CvNuJPcH9#1HvPy)RD9=wn-M5T5+3Dw3}i!`8P&u z=Pi%B>;a-|mmGEpc-b%=pv#`ZD`l)w3qz^!)HGID#0xE2;aO6ML zVDLmn5K{LG1OpyA46xPJNM3Ya4B4268pJ1tlWj?3WFcGjDrpg~HheiRug_xiA@vJ+ zT4Uk$2g_(7_|>Iwc<3h_-Wb8*@X<>Ba?ZCtkMoU-P~_Gsat3j@CWiB`8_&}%kMnvP zXLGpT#_5N4GC8K`|rcJ2Xuz+ri6L zDrHCYWi7mHiBk5azHG=q#&ecZwn1MupO+;oWtIA}zhFZIU2ajzmgvhmdD$?f%&IRN z!plOHvKW2Y{U*jUKq(ufFWbe-&fd$cCtpMtXl?)N2v+a7Quc|y%nute==ru%)}$|c zk(cdO%3jcy-Gu-i^|mQxRjf?3?~M@z))zlvB%^$bQockfzm}snDd;g{ljC1n%#KB*!;ygu^@@mMR*_^#svA7rO*Fm-{)aGdUyF`4STt z`DE;2Os$fmO}75P{FG)7n(Ca~V;y$3>NsnNm(_7WZv77BN};ocsNGYDFV;q7G}!rQ z$O{a#VqI+gp6F}h$^9{(gG=m%WJivEVnjRG;5&P-IAWi;_FC+&&Wm?6QTekdx4PnH{gL%wc_AOt4b&J{-0KSX7l1pj zqnW@J_q~E^MWaZeZ{eQcZl`Grv%z`st|mDWWjkpL5BgQhkD!LSX;!un9$x!s6X;szdDWaz% z1JDol3L^QiYZG*V+V&XKkPC>^%t`M8Yen^sfF|z#3U`|Hd9@~a9LhjOij|`acV5G5 zTNK5fMKMX?%Ggs#N+3xI;B?O{)1=&^kSYq%$|HYDEQfH?QZMNgkdi_VVw9j`nfY0@ zImUE`LO1-0T$n5N^?7iiLl#)zR&b$t+aD~bJI=F#_JdqPk}w|;%~-FbPk`20iB=F< z(ve>6C4HqkDXn1gP$E_I_w$mj=}!7Mk)9(Z&0K*S3N)+C1u2?p=L_f@;~!ZMQkON! zyHSO@$Q9RIhG<404s^Bv!~k%hW?rR8QtVI5zFb9gii0{)QR(Gxfz~y3A=!UEkGweE+D=i7 zoBS8mP{%XFycZxE%AKAXYv>&3D%GAh9tX z6X;H?T!OreI6G@tUH`jecrMOW#mdkAAPBJ+8%YLq_$&`h zE`b6&FXsjxiI$LYMsUU?UXe@KP^}@)P(waG%REXAyzz^0?dz=421G zx7u060$~DX%?8ZSNcK?@Vf-tzqJ1_~jZN!fr2Pahdybc})y8S8Mi-(?K*Lb)QIxeY zdL&yxFG%h7qm<&)WJAHhOsdb8+$my;oZ1zAjsFDm9S#fF>C|m zBj^VJ4|j&cq#ij3O-^>SGAo}56a>{V3zW{hfO1;Kp6i0)9BpK@*Qh5Q?efcG$xH2l zPy;5Vl?{D!76#ra>@mR8>=(({E_*x?IP4LWxa?*m?RFxgkK|xm9JEgWp0|$}BBCck zM$&GN(aMMyv0g|n^;4Q4RM}IJB}W+5m#IV5c7}ozykVL$fE4;0(H3nfu8-l`5j|R^ z^6W!2Y~qMygOI7A7QX(C)&#inY%oxD5$)H&c#^Y@Hf$Dgd*el<;&vAW+_6nN$=!F+ zpQK8Un6wXzJkf#eS!Ok!INGaR7YTOQm!njEV-lyYOhH#+7zvQqW)Eai+wCVnLiR#4 z&n@LXK7^~cl{GBx@nYH+Z3aU|=4fxXZ&iSH`!+4tpyl2~F6g7{$3s3_@~%t}1gA0l zN%Fag=m~HehvasAXiSdBJc;SIjv@{W2t*eq7WuT>2}3g5>@q2dJy!~hjHslg-A-#4 zm9^VxA?7)<73JFOZpyXUPxGXMl6E@<)SR$N%dOLL)k==p`7vN+DZ1T@%6;~Z6T4ZRiyt+qNmaxE-MZ4HHQt!Az8#~3O-C2qIRLzBqu zx_}t(Csu}BgS3w=V7#HUwcXfJLmCT0$LreuTnXt|B)1hK(jAL11J8xv3>1`U;UkL? zacmM8DBpXNAh?fVITo);_&psphtu6bbg%YI=i;Xo(vyG4vx<#4XgD^ux;uOhkqouWXwqiH7haOefW)Ryy|Bfx$nFbkS z{}!y}9{kVQZyl%Cx>#lJN9-*(VkBi4}Qf+ia1F*CCULL%hTZ4 z!bws%$wD0o%M-~%oFs;mBSY8tx++Vb)xnVSMrKdr22JzHSsOk-A| zwMw3bJbO_4PrSrEhUj_p=A7_ywc;h?a?yC0*TB5w1+?CMhAq(DrbGQ; zf2bRKtIGb9C18JbkL;UN_DRIPn%KMImXd8;6P|qp)xL&(Wk39JpM@X&lcoiClYMZ# z(zJyn`%m0?SkTI+OBw*Qbzx204_9>$Voj^3Ia>*j+U2oUHq0wq(G6GArmF{%1*SPV8N@9e2-YE}LF zk^X52`uO^&EVOPqKcaOh_5#I+1yE%d9nAs|cOR7(5KOlh0OvSZm&&&SC+=p4g%m+! z3LHEOBOK}Q_t6}rb;}{^ym(`?ILjEtP$6}EYF@BMjMyip4eSG~80}sbCq`raHihp3 zyeyD%4UB%?z$%uSh6;@Jg10dzzk~j85g&-9(~_ZCjOIp^EqF_eXcp6qe}j*T(MQF# zGcTbGJIW!h3pD<-Ie%k>!hZwOTF&sseIOk!vVsLriK3$(F#z` zA0CE3!%spJO`*$yr=z`Pz=BHx>fM^V~iy_l}k$9=w!9 z@!U}@_lcGxuDq1Q@SLpWI+dLKCW3M(dIGJOvw)tJj)k7YMclp&m&PVSdy-AslN8BS zLrRhM!~)tAPq_?SX5S^hcY+VhSh8SU9??f0MRp@8jkJ*(d0rB4Y{KkgyaLvTlh}_k zFXt9VkH3Y5kNnTX>V!0IMlt5RWy8g^34dc{rkaH)6C>UOmwjUNp|VTFwD8AJBt~zf zqpSq+F&r0bUAcu6iM@bWPnO0sO`bY{p+f36p+1RW~0b+VG>9 zTrDo@Q;0eyM*mzE0xcR*Bu3YhDDg3jeN&C%clE4Mhau@{UV9658VDNSVUo^~gg}N0 zY22Vlnoz)LH*!gzaY2MMV)R9Et+@}=uSOt5+Tcjegv}db+R!0X zF@kDr$zt--Tf}HA8mDoxLz5L9Sz;MX6rXGoqxT?b7NdU@*WT5iF?ErS=$a1}aT(MP zkkJYdix8M9Pc+l_h|y=owPCClXKHcQTmaLXq*P#mOjJ%l!Yei>ic^~$4B@O>!b3M&UBlzR+UG*z7VrJ^adik(n68);|8SdM8V zAw=|>j%Yl#e$IL-;Y9T$;DP?-Kk8^E5Din2(=fT9`AMTGZ=};5oIu?IQrOc@&s&@| zCxArn7;9l@r~MQlXT3~+*|;tp=6Jtfi!EH-UPlL44i}wnIO?nDuYDc;t*OS}sygC5 zjO8iDkX&_UtgMcYe)vpG{%9DZL>P}y=`F{eAC8I1@RB)LvO-)$rbZ=>v%~DfSvj8b zp$yYg?ic@kmUGQs%Q%Um>eJ}7a1Jq zBnRfG=4H*ZQ$XZ-;Zlf}dgS3wY>IL(?kq$MHRNP7J9bImmb6 zdl=>QGysv-2|V2x$kQ`amu70zVPPFo7WC42a|eKpDIg?<5iB*aAPx^`7#{1yIJh5Z z3tE6<6_K*Xl?V)0VOzAD!gAnP!*R?+L0khS9;{9^TalL+9VZu=aDOG1@iO#Ej#wCX zbvo-?6Uxz*Vq`t#ItJprO1>2274ZS3CODwv_=lWH@I{a9E39_JyT|J<&wYduU_APu zAmBXBUfW4lj=KZe(Y7Xo35A#QsDMpRAuI%!U$l^(S~y~YG56!!M1jFWDBGQdhREgD z6BP?#*d|2wBy_7bkYTCLS13@TThT5fN>!gH9*2@eV?#wQ+6j#oaD!k(l|IHulo>OUP80M;Hbi2Im#e_td3>Bi4x`9faPh& zUy5o;v0d)v*f1*hI!$az5~IHr*Pac+nQ-j>q}V3+74Qbyf61emR!nPVnPKdSgBA=7 zSU~eXo{7Co!b#8iM^SYrc}1I@vQe^$H>`maa@I5TV5^_B3Xgd!P&P{b3`I&efH!-} zDBq@$Kkg-`Y?QpUJ2^SDe3eFC<0YqTl)Sn-Ir+WZMV4gRKj9^(Y?M5sJ9!w9@6yPh z^paCHO1{23c_@*u*T`$VQ>(j4Eork;HcB3=)ujjpb^oGL zRM{yjU!m3gt6I`#r)-q`+XrOU0$R+JPC8E=L~GJ{IRk~TAvnJ5tw-4?xsBC3bBddI zBdu$ww-+X6X94mJYE^c!#ntjIL{Yb*u`+z_p)hflG5M;`n> z%7)qLVX-t};;p-)SN2sNAye~C0FAWq6i;0;Pis1nB1EM}%~-F9D|BE_F-RAR@sPS` z>>;h8$VMIysg=jY%9r~x%W*qK5C0N}i;%!*kScXdjF-4(4L5UR5je6P(MrA}s>@@D zqvD!}Kp{q)z=_Of_2C<9UiQgRu8Od-lotm2e??nqF;Gh$#1 z#5eQsBwltZZucBwV<@?i4!|pqmj+VahCEW`zNx9^6TpbP-elG5HzPr*i~AWruR!N) zB>$4b26J}Y**FW#JX?7k#)<0k=$b!w0&EYZx-x%8%^0cTQFoh9-EszpusT$it?`?C zuG^r{5)EiI`9++koh|r9yzYcwQc3p^h9t(+Kpey*0!`k&rVVj0;YoSmBUSw06g-78 zC}-P$l=oxhvbg49rJNcG{I@fRAMj-qS8KiUF3@sZDCVC9oW9$^DEJPw5yM&G%^)N( zTUndtyp0JP-s7re-~t>R&!K1KIDtO|$2y2@5}nVlW0mH#qT~p*ksE+-d&G#-?vr@t z#sg@_KM*FD*R%WE^2T@dH=SyyGtYV$31}oS>8zY~0Bu0@X(h#%;oPg8UuMQU(DU_y z1Q-E@g8Q$Sp3#gNK0lbk{m?pSR+GZ47{ZQW-7VyKnu%V9XUm5nk#;5V9jw0MxMJmv zd?r(!mbvHeoR%GDs?I7#yf51>Va8NzG2iq0Yma`4YQ%xOLyq&QPDroIx_WX|lKNXn zcd1G>A8%DQ+a~6aRIQy?8=ML|P$!c2!i20*aI9K3;tz^zhN3eGuQ*(}iZ^n+a`TmK zz}oF3lsZIz&19>B!`{w&DMj9Q6U`z$@9s9Ej2}7w2{Qn?Y&mq}ySr;gb*tvx-377& zbyj-xcJhB^`ruCS&o70zZ{knr)d@Tj&16$Nny#$5eD@8X|BgMh=;V(qU^-T(a*wD? zBD^;-cfNlcjm^OilEv26vw8Kd!(#Nndl=$Lj@XX_`h^U!fg_rBmBWCq(;pTiVUbTj z3}JHAJnJaj{6pRSjQf@JqA2Iq*?PkP-r=ONJ8U``y!t<#0We~VGm9A zU~VWv6;RULfM+S%+`zmS4^7-{;N%_F?uH9E94IkYyc-}-c^i6#($=u|XPFOPN!_nL zX0{Y7&tgBg75ZN`mdf6WzeJbzGEJXG+3*(?opk1)zvnqDg zN{$;!_O3et4({q>Y!nQg&zffnpM%u9;Wbh9IoLrv1#xp99DiCg85sakAdc>A8zGvJuR<*EQM+PaH~r`u#XP7*D&PH(HKb>AfHd+ z=R5|Kxxb(rkGNbX5$S}#kKyn<51q*`vsEH~cYjGVuDG=dj~Io=O4a!9QjpNQ@fTjMH~w`6 zit%4X|H|LR^<;c}9MZhco)NC>pruvVQ z*dof-aW@UAf1=^8%k&zS=PMeTNW&RDYDjYh)9_x*j_$oM0QC~zJm6$-h_Z1aT`)>m zJnxSGj#KfykcmB3R`x^aXVhlIbH4Xk#SjZ6uppj4)JD?=qG|3p(!doLz(&c}@-usR z$C_||jCE`ZEk(U|%G+!iSEucbdEKr8Uc_0he6O;6<0_7W{i2VSJpioP58`mjeSocY zaesk2vCRx9W? zIMpfz+^kSpI8~W~uHjTFxJIFRluOK4s0uk%p#m;csQPiLg$g>EQ_bNZe$6D`3=FOj zQ|N4OYQQWdh?ad-40bo8(da~FLx~4}W;B=FF%A8MK1-OabH{ssV8ra%0aYS5Q9R)c z?|tluuY}{U9WiPTKyCMA)Ium8(oC_y7w!qT`!0XChdCJgg^>GW=zxo|?&EKfMJGhy z9-myX8&oTs*wk>!-4AsjP`lAUfvI}~kg)VfbUJn89bicva1!lfp7b(YO8Hq~a%LMs z6n3`?KQNTnT&f>8(%s6qIXVe~hm52$-?g8f1u_Rp#A%m%9aA)}4y_}}AE9O4B=={M zd%c3zx10F=xr+EXB>tJ$p2X7tW#aeH+15ZVUQ)!*R>Wti^V)v$cK&-xpN6}zFYvA> zx2uvrON8WWdz4H;0h26|7>&_C4hGIj7q_A2?(Zjz7S$_~3@n@y5m zis`A%vsKCO;_%6R1xfBcEjZb+EuZfn$61>Dd#t$7ZkD25f}-6Stk}#UzG4SaEPnK{ zODT3RUc20^YS*3s?fOAGxHLPMbNiwDA>ugHCIfs5FC_zpX&+!8BPSGb$$)043SyQxC=l9?;qMXEeI zq9;qFsFr9!pym#SD(Hht6v=ZG$xGF4UP;uwTf%|Rz}wfisp600TDv^7NAVQ!GD}!U z{Cb2D?6pmf7WV__1vP(uj7BY zE`J0zQswT~Wi2f^X#1rzJuB;S~Tw*~|JX1?Sa{2;P!BhVUsY*(`5 z&;rSEQmQz0HYE*@Ny$Gad=6Qj!F2EFJMaAf;O7u8mvIIqV=meT<5gZ?Wg?FoYyCVY zl;6F#C@JV88GYP!l;Hgrj==TCgWj@4>965G3}y7jBa1HRE+o$c`*&!^F8ebR&r3B z*&^PFqdik!G2)mwYv|ul2Rng_V)SQaGqH_7fuhUUWu$p~ap$Nk7V~N!;IR>iyN}V% zZY>*tD;6aKT6nv~=zYcCwfBK;5Z>HUjnjw^7=!sO9edEO^WyxEAgkj~N_$fAGBvaA zy%d(P(AXy2n_Ib1+WFoOsb6e-X0I8olN?`2Kj0e2Mc5s-|EaGOxkt*~Bm3jr0+%56 zNRjPQt|wuV$iVk<#60{IBOt6JNS?b>ySMNUx&r?)!TjC=WNA0-OZ(s*Zsxfy@*6ua zZ|ePJ5-zs9FFAfpcAQOiG$%U_Csur6NN#pdOg1zpANtWc=3UYCBJ0y1I)+r^$_akI zJ$f|^7Yt(!c-=Z?-vd!}bmN+Zf`Ch;=9B&)2mwJ_$S*{O8+Xg-5)Y$Y@xe6vaa*qB zO2KKwM^f{diIU+X=@8zA(zNP^j+?5%1`|U_u;e%>HOnYGC>?@z_P{jBV=g|h1eL6= z0k1O^5=B!#y8eO-&mn!3{z1VGNGR>JI-2MnL2|`uKda-g)O>oP)o@rk^piB^VCibf zWf5qzZ(U7yXbems8s*N0Bl3Q>I^JbmX*fh)#ETsg#D`xoAh@5I>^PE`W_2wNz-vi< zrS1)s?1#ll`VTi~(3}@nCP8W!n3@*+0zr28gvo@5J#_BW*7T*=cItC6`e2z2*8|11 zZ;~V7wqlWZX*Lnw`3odk9ba^aTo0gb3y>;W*gc4ORqz9#!^o#SbyE_tnikMeuC^cEi1)wz zhUv+1REm_j3H}U`)9hWg2wahI4}`T)eI{x$A=5}C<8$XP?l=@mj`J{yAH8$OwFEQE zo@B?~pn}70?DV$EO_$6S~VR~QT-1+o7l9m>ovtxWFrX@SRqn?v+Fps{If8ItD z@XSD}Vs(5@TpdmGV=pAmbEnjMJpaFs-)FXWAHN+0|C90i^K0G4Z(IZz;jUCb++SmxfUv@{+D=XPV=6?oaW|0dOOcd$M>m{<7a3T z({0X+h;3@Kgk90c>dFcWO{lVkg-R}4*ws0Xw6GzVuh1fgHS8*wzgpaGh~48jm~%*W zwCAk;!LXs?q(Mw-ckIbIblUL|!)yZLqyJ+hQbn^t!W?K{1T~%GcKN|Jr1#bGD=>p| z+|EEC=xqomVZDUPE*mD=i-ERlC8s4ULYsa?dllKVVs8w0%ftmO@*Sv<6nn}FOPKLv zWj^i{hPSy!-+Nbs7$^-h;9#D9 zPkrd;5(3YPm4m@ha!sa}?gWc#`V!>$2h1?^JUw)wMdOQKVKOf-F@%Qn?B69qT9C7hm`0fSO!5nc3s23g* zLlV!5A$OdR#Y+ZCBsMJ4iW4{N~}f`>Ka+yVJN-THJ((sgp-7Dl1o6+hmiyl1J??_0lfTD zUOoureObB8riT@N!MuDhFTV`sfvkL=Qf}hqm-BKA`RV;w`6iSrDh%NyS8$RcAi)Jz zw85^h7{tr3H9A#S$}6#i$Ii1{KAj*`<}Vh{9w%6vziQpn&`1n21Af#aY$25ggt?!_);<>D zo&}T5cIzZVl{KlaxMRR1q*&LRF!)+ZzhdvZ#dhbqKVVN{{2OM$t!OgsH!zdAPeBaH zr&SEWor%qd2%ohV%2~n!W1FmwKS=a4usSjf);W5^!uN6mnGU9)84~XTTXq}%8cO5D z?K6d{RNNLG ze=}cy#O)y#)xFztR$ovS;5NPhf5AMs|dofNNNk#@|3 zdX%m^%Dl6TBpi7_)9vlY8A z;hmM-U*RlM{v52Cdpla)k;>3U5aJ$+e6<#@&Z3?~_c<=mR_<5CtQ7eymzKQ4!{}iJnGu7}iXav_#CWwbl zb(CoD57$IzE%$a(%Wga=!+p9JaS+6BV_+_e?P^az-@zk85)AT0uuFEd(>%PBh)Gtm zYjl(pd0KuAvaz5@t~8?{iMOAYF9ER?Tk{F4cCIt5!t_-**b~JUlo&BGEi8uru45d? z^y`U}V79-i_;pZOtyJ;rVB00y8(t4=#DQuICYhwMqr*yN+CmxpOHW{DLV>`gY_h}@ z8GG>m-iGA&JD+Y_l$^vdmat1jdnB9I@vhokG`8flD_~A>ds>)3{#wEU)D}z5ys(7k zlY#vXA`a&iMn=&`Mf$O72xd_&P(>m(1uw*|KnAHugrh(rTm^D~ibOOFBqCHGgHG^xk|3>m_Zm#D~KdF%_A5qBhE8!^)lV;4WCv4)v2JsV4h2?9-@2T{_;VE|14 zm4$NL?wNrO14Re)Ycg?nQjoZN<^as|bQ0B1pAl?}b1DBYV==g;Wk=NtG4ffmEft|8)$KS9lL*S?BV%r z9;ag(_XprabbnQW0DaM5-Y!i?IC0EHbl+r zq|`Jt7g;)uADn`W75a#c;=pH56gMfj3_VOWB2MNcVU#6N zrW#`{fI#{^VG|hLC<6mK;V0h~aX|+T>+wrqyt*I}W6?tIpRkB?egHJ@U7Y&kcEBeP zB1Ye46W1Q5-0mblB5fj;7l`%@@jQ#8S7LV_WwQ@}jhl+xVBAL3ZGdLWN#ckD;#&Iu zn1RPb1iUd6?V7tBa*i%rE=EsYv8)(39M9hXF>C7KIaEDSOxp{7_|bX@PTSGMigKut zh-V36#eUEbi=lf_1zd0!z6mHX8kFLL!~stb#90CGp!uzM$N}b05c9Q9Q;{4;DxVwuHmsT>%H64$`~B#7Inqpc?!% z6-4e+It3Wt4x2Pm?IP+j56G#jlzm>ezRC|oN1?a=rJlNu1|I9FP#SLfUb?X~06bV< zEjUko^}p<<4`HI;caylb5v`@J64$=Qasw?;UtF^b!2hbd9wn`)y9T2QbywM?ysIi+ zAXeIZY%*TNoJhaSO$All831LMz*0sxa_D*dwC7GfpBRrTn6gNsJVcb&a5p;uH#?ss z-q8h#^G`#yBicE4YcT8Y$Ed&QH<`S@Nx%v)Ai3eB<}>BqHthlXqs76$bEl~!af@3* zD4^GWH0Og~p}={1h$8nq{897*Z@B4R_L~)x@dB11XdrBJ?2Ro8?A;+A-WY}L&Y?m#BC672)2CYSyHH@wcJ{<=EfzHse()K+x@5x@upu)m z88MA1hr?~;^Vs5v!++UXrOhnklPYH9h!2Qp1^NdG_FCMiWGt-3 zFkq;=4Zh+v*}tem-|jk8nYnpJ|C>5I_V4IW@oYCc1SmRG^Yp)Lhi%@j=~<%o;$5bbpM`rkaO25t?cEpWlU28c&y(OWqk772LLr_hQ!XIyR&-X`( zv)JJboxyoeaAu3!(*v!}n;n?0>7Xs1PH}F*Ay~Kb87a1@YF=18j%LR6F%J>wRAvH` zt2>AJ??2PzhXwz&!&APgck}0t^uX~JoUXI87&#s`0iZm2{7okNS01+o(MgP0`4U_T zd+7Or)12Ap%O9DoX!m^;Z}pV|S)j^74Nt&UNPFJ;KdIdZy0v?J7;pH*C#d1$!?czc zz!GRVf8@%}plE;uC<7!8mhcV`+KacypMVA3r)tH`jIabczzbyk&kg}J-Z*f=MUn{l z7EpIPbgBcw`~3A`96AlWdkBrBdD!$tVx{cByFBO|S#ANP{@m$OsH&bj^<7!*z8#}P zo(>{BOd&P!gwCr_0)`zl(>giwi52! z{U}a>X+m~{qOZuCPp8mR6E@^<>kI+}_TT32cRs?Y|L8PlmF$$$ryqXs6s6*~$@Ev5D+%stM-3DRa z$9!y*l!M9dp7UJ=I{kCL6$Ha2167yIVyi*n(8=`U6$}$R?8Bdq5M^H>KZ6STaneDs z=8x6xFd8R)&?bI**3!|BXIeY(WbKU7>QCiCXolK4xi0`ZNSX+j@wz@6msA-i{_KuQ zptyai89xvhCOO9;G%-%N!JYzBNpN7zmE#Cc>avtUQSxzC_ILG06o-57|=iBlbFi&*srC)Caz^U<4Lf3?G zU1;wG6fo%54nXl}J&OR=Og7`x?V1=}?gwb3<99<`N5z>y-n!>`5S7}fC!dlV#7#-SscSU-lz9Ry$1CuePbQkG6fCd z!&Ys}F!@g)P@YFOV8)@(s>GS(oAd-e>=JF1O6^n8#aKZ?`X-OryKDiehMDYZdh|X4 z+^4&u%y=@+t`H(FUy6C5UH;BV;+JCMpxp#y;~y`flG^(cJC&^u6^5XIa`_8QWkkW_ z*sP%b=YX5Q8y1vLs3i0}eH;d`)WF-}nW!$A$vCLTY;zvY7AiSsAwca5-?K)xCOf)t z6Ouv+N3-OdYLX(^Mb|NH;+nsZa$~*~AG{kZtz*z6SrR9=Sq-xI;B6#@e&7*|OtQnB z6nh~l)?+pNic7_gWAq3|syJ=1VVZcrPcj^)w;1CIrWKtiA0~@nyJE#oC2*R3zqq~{ zf~FC(9mrS>9XZKjLHjfe2~)BwAf4_V@1q?_q8I|-=%v47_vbipb?H{zs_8J;a0w@1 zj^ua`a~ImrbRZp z+u?M{yHwBXID(FUegHcDG}NVuPWgYZ;Xf@f&woDuvA};U@E;5Ozit78Kw*_2WM&i; z6xz(0#n~CQY;%b%YwXyv>`Yr>u{kfJcyV^I*|sF3z?`44JiR3MzU+ud)h>%NN^&#P zOKiot1&hayO`n#YSy%w5G}C4-D9y_=6VtMcyxc6MLePZ&?GN2n7tC`f4;vPHvw8F^ zi}@~V;^g?4*f@)Mn0Z=m*6>j`#iW`$>fY?E+t%TH!Q8o_rekZlx%0V=oi!UeHos}^ zdU{>g<8PWfozHeY^g@^Y+4_g*^QH2Rzicyita%hN8D+=%N^?Y4O>Nf`8zaptphIQH*1woLYhUPk zyt32oRJd`uT{V?mm0LSrT5axl_3_SUE61vg?urm*q)dx0+4Cy#fZAYVKKgcpsDjo?jAGVuQ0q<=q>Vw>+wNc~oY8R(ehzPl{Pm z%%S4qsGLPqM1NTx1;_)RkzYgwEa4D}2tpaLr$U}8Fp3$B@~qqvTYARwyzHpV!u)(F z1l8uAezo!RYa8a1Go0ShVD4JCuCorl^5nMD4O`(dFLbTjM0W7fd7a_Ot*2jAX-4S$ z?ex}aI0$&GA@AsXek*knoXq)(x#O>!Iv#r#vKZ?p9O%;yXBE751UHi~7cSGWv39;W z)ne^lwR2k)RO))_ah+Cry*6)yo0w-WDqL>vd}CABLk%6zztHi=%@KO}k??WoMS6Es z*D@t*9_p;A?R==dyBc0S^wqnYZ3vBFo%a`O4|n>dB5l-73V! z#7Os<(X!4B<s>HIimAV_KrWV8*83~fm7M_(3|Ft+Ukyn z9yd?673U3`9HpBBy4Gws-B3Q(JX@iR3{vdtmG7M}l=%VH?RaRTGQ5Mw6W9xczN5i^~4OQ@_ zuGO32Z)jqbx%1W9ju!#F@W_Y}a3ZgHQ}0xLnvDD0Msw#=mCSlfo|jIQw&B^%C$^c% z_G~0{JhzE?FYCF^4XZn!eUrH@>jH9~C#&>c6ExRrPwrrfy!mD+qaZ6U8+MH{r%q={ za*ASQ@*l1)j`gNOGt?N`-yI$;uwNTwr<5ryWwi3!^bZ0FMr zozG#a6);mr!69iP=kqsk`T0QkOxX+_skn`I{QJmHwcIw69Jt%iG)H**!G~UFC8jT1 zm7aS9z736N3K#e?%+}Vd?3|3!Jj_`|McD;eLf2EzVj$5B5TUj|68~3QrG^)7pDx@! zWr{GD&EID0d|~eGx8V;5XCRlFc6-XT^M&ZF?6PQ^?cSNGlVXJY+$^l1CA5;~IN=QFbvh8JU^cc}TOed3twt@lt>#MY#p(`g}%lX8NMs462)tqKr&i zZdrDEaZx_;$Sqz}SX_{uo>^L4oLyi`FV3*#77`a*MqXZeS$3WeMy1QpSI){_R9IS& znVoLS&BXs&I(&Fu;bQzmP%vO?{ zm7ShN%O`;1!bOEP%{H9JlI+~YONc!Oi*jw5WH=5jPcO{LDaoc{YV&=RA_SwfpfH`$ zz*jMObXH<|XR!)KHewZqRKt<{{2-1{z5mt5&IdQ2-ntre=9T!PL>DY5Qi3HG^q5&# z)LH*5Rz!2>8W)BNje=;JuCQ_d1CT{1K-`LcQ^7s2!htb?JXejmh{r7`3J(&f;wudu zFJd-l>{uXTj#k1Fvyi9+FrY+0rVc?q^*8#819Wzo#$HytNOe_W|!XpaMFp08RPCtF>Nk@W|22M(nMVK-c^Y+M`{1sL; zGegOk!%GDIe+I(e6ePP19^$#MgO1#G2fwUZ7{waT=mSyO_&J=|E zET}U9-^QGruO@=Wgr}c+iqb};y9Ht2q*0?rf#*}V2<78i@l67+iTI){ArXv_Pe>Rw z>a|J_@!T`Fy!T_! zEm_XgiO0qL34%~G>A0AjApGW;l9Q7|vTuQIZ$c;N+%`!Fvn4}bJY*$&{?qlK$ETr! zrzh^Abi-3meLOMqpl#yrS88rCTbbxucL)%qX+O#>ORo>U5{rp^t$+-#}fuUYCk}Il8ZEu=#F?i^N`N|40jULYpL}BU64sXMd9gT0(Js!_O=pY1}rUZxHD)wI%ST0N) zGWNz%Vb=l6>d!-ZvDD*X6$yzC@wMT57WfpZuY8Dxo-*t^?C~h73545#?>6W_*9quD zxNZ1u1@3i1;A6&jAHMU@Z#77T#9&i}pCvfP&k+=CN(>H6?9b|TqUb1S%xE8dFd*<6 z3qd%A{>A?ZEC@4#O)kHr;LvscNx|kd0hZv13M1I~KVb-tNDMY728T`xHcbM*mbP~e z2m;w60rhO~hZy38(shs#M4iwBNS^~NipzBS)2++_SmW!ZSjy&fD ziEI2V!RC8{f3jbs8--O(U{ca5ob@@*UHm?gv431c1oE#ie z(PwJ#LjO=hP;d-TXuu?`0sX0O>0`Jr#maiV?=xRsBM4Wj{clAXwQm!AdjNbX;BEYY zob`TS@-WG91xyN#SZ7QOj#<+uF*v@WuO+z1|MGyK;CP@Q4V z4}QT#6@AzAS!Z+wJnX;TPoP0xjqzgH=kdIcJjrvBJf%Otl=>sVFC(aXf85qzNCEjf zs9Spw{tDX&w^QAzR2TMJ6ChFDJKz$3Lft7X!RE3e78w zYXa8!yTFRs*^KdOJA{`4V_p!ZQ2(+%fiWxK9}E4>ur4_UdIydn^|y2fncroBM+(|> z2rTGh#|$_ zuYa(yousvSYB2mGr?<`oJ2Sp%;JL!)@n9Z-9l;Zo^aGs6e-YN7$I+G#so&=GFrP5H zu=!+aaEf1ZFLo*GX_k>_2*3ND6r7@$in$k}sNdf~zdw#Nzy;x~y|CfKG}pTVem5Sr z8vJKddwZJ9{J*A=0!=Xr_0wEZe`beynA#pkED}X_7ztUyTf5nTe7_ttov_v0380~C zj?DKb5A2h?-v8kMmvLR6ioX7xmj+=uPVDdhNZ%1A|G)OZOT^5m`zGox1ple17SI7Z z59nh;i~ZL1aTy;DSnnSSnxj}m=hfps=|Z0Kz6-Mo|9CtG)2-lfzVEAs@*v?tKjeW6 z2<9o}FDpadVK6>s6rT1!Zg|KDq2AJ<+waCNehwHoCMap)Uk$?RDs)SFok4g*friYy zM-V}mE*h@Ia^h$Fqrb4w)}^L)F(AD*(N9{Z&-k9>z$X4>LXk=o1cI z3^SftDZCNRiarWA{=8Cn;)bC_{$#|Jko;gID|s%`xEr|7IrM9!@%$=b+l@g)_vwv{ z;>R0}Uqkz0Q=nj76hplpWjwx0_%$jL+P00}VftvLurui!1E3Ak&zAyvasCJZuVsAX z2VhHkw|FV>otS&RQqvTW>gUXi~ zUs@^r1AoEpr)9?1RtiU#GvK@B#wS6y<6a9DJ$j$h9^;odE&8J5ye_XLhzTJA*ru69-R?2JLt;0NRf zgBA-BlYp`jQ4nC_4G%~VAtn!g01LB86y*VdCW1tk{E&$hLqHGNow>3x9wy^bwOX#1E00&lCR_b@tyZqo%d;Qm8@ZyHEXn;5^_{me zATp%C$8hKQciB57OO!eLPvL(Q_mAVWe2eX`Jlk1jyfv@_%<~gm{uSc~jF-5BKMG26 zW{h=E$~@Nr?^VY4M^;Y767^}}f7${f|9=?2!}vDy)AK%(Ux_SWp1~0D(?sW4Y-09y#A7KTKXN}@5@3acO2{JeHhPMq##JIHVjce zW^j9*{#hx&FBIVTEi6;(?E^n?{2^6h`yq9lfa5mp;dwOAhH-NY{*o}I=^Hw)yv8@tD8^E(Ut1mrh(JZISO zH4FH!0jKug#n-5mq*m+EFuk2WTY&#+0j>-1xB&lS;G~av`}x@d{?~w0zwUg(_KTh! zQ+n@vrnmoe0sdqG{;2}|CEzs9+q__9oWBB`+H?DNZ9B|61YCZ*Kn{K{BIG=zW>{a0 zn*;wT9BfI>F3%U#(4=1hK5hM6F2J`6@Gr3Z(Ti5@o+av=#D5%(Eiu64jRHAuv7EhU zEu(qggUfY%Gd``In+5o80sa->W*nZk^36LPT%LkKm?nP%IPvHE^#=v~|5$)OG&5cP zdySnu>Mynm_@67lUn;==0eH!FHS_b|fz!T1&Lwj5pg<0`2vf)BVc@iGud{s+`&=#H z*9AEJftG3G|7-#NVgdfS0{m}*(>zxG(Du~4_rT?U6!1TW#t?tr4=w|r*3R1iPtcv) zR=()n%YZ(FaXxw1;-c5D0>{52>01T(w+rz1;2=Zo%}+l!KFIaFG^8-1O)n`#@O zs(KJ2=9~hnk|+$y-e{!swQKc(+aEfs%U6PKcMZ`XT}OL?)9to3uEDU{PO8Walv8K4BE+@2?lGf1XTzBE0v0aw6(U3QfIv$BDcc%&e-q_o9lYLq8+Df zqS!}KI4W1l{Wx;V6;y*-X3^Zv$Hs^o?)mPh+3E(})dj6Lhhyz+Z_jBRj^gT^ZYF-D z13!*+A6KZ&$ZgN*^{QU4>5Ry;TKB29F#_y#5QawTJqm5tsl5<-C=tCnr>Qa3YPFV* z&To1&N?s3fYlzY7Cvnqr+Ae-LXWcmNZw{T-vX_zbLmt!AIMxxq7))!Bvqpn#n;qgi z0^c3*oV3rTJ6Wx?{iyE-EezWH%?P8s=5IQ^BuRB3&9fjpF__ktG4nR8vYB8A5={7{ zajWm37?w-WvFq@D$9}QaY|moFVeHYYS)E#WRgze(ULuWHiV*A=RV)2rKe5+Nwy$r` zHfdy5WQ`Q&3E%H*Aa2uH>$WR;t>3O{Q2a}eOQwo@DrT!B<>ksRu3WxwensneyrG+V zQ%B?B5Fd^h-CCK4B81dI1=A2GWBjIHPj&pr-$a}~^eGuY>%*ZR!)A0sy%~fXZlF6D zomji$ZG|BIQQ#+jhm>_1hwehUcB9DM(FhWac2qZV2MEs`4+c9ZlFC6P`-rnmv1#OH4H5Vc4;_4OcJjrFATv}6z-`7utJ2AzU zuA){&!(xpJ3<&I2coW*ciH~XgC<-I2hsMQ~=7q(NepJtv>zU$yFEn+vJX>BsQ`RpnN~@dem-OL?;s%Z9y1Nnhs_PAtU?Qzno>OMH zAarXWP{FlS9T9R(OJ8A9ofJnS22{M{CthTtpAMh_Kdr173r# zuPu0P;4Q8#R?oJUdjpki(2?dquM!Hr_IfvT*XH9ft&L6rpl5;Tpz53@4?_u(GaXI2kIsKYZ3!lCUBvXQx~xYuHZeUO47%uwsM=h0jS$IZyNtQQ2-Y=zCA~;r8JzTTiU( z)-rr%s|9kH@H8nhRn#Mi2`^<(+Q#i_~O@E5oV9|$j-C!K|vTada1LlY4{&xt~3VIy*qrmli zXE4BUWlTc3W_(pUTmV~t82eGu960dh;kwSuBFs_FrJcjFL@cR1J!_ne?VR8|{-tGTl2YfN{jv2>6Zyqm%Ch zrsQxJEIQZidQaV>%3T@W><>G7&5z+P8hGafpGP}xY-Z>CVXWy35=;*EK6$HccR+o> zYi#P^b-Ww1!|-NSbIVnuDru`Tk3EkuI*zAk&z4aV8ius?bqci)i}haI}4x3rF18=b-Cy#=ePSthYNK}il4k6$ax{Nqrg|D+x@)vBa?o=3+Ei|G2O@~hmkRC)&QoD4)f%g=y>uDt-bL& z_c&Q|X77?+LA>O$$DGC5+e{c~Ty~izSZX`8@%{nJ6wTx_Telzh8{_WOqg|aw#JE~I zM>nC?q+Nrs!@Dka=&~>2JdJgPr9^So7|wSzTM}b%Jd4O z>BNGq*04b|pWM?(V=rK74m}<=xL0sgvs=L<$LH!fSk&y-iO#^Tzy;e}Sn;Wg0!!C-BU+U;vm~%A z=%es_*9(lrQVqudGllo7$>aj~u?K8bJ-baIZEjx1&as1!?#40xSnJZ&a^!c*)7&n6 zaXc)TN*YaaTi^-$8}xe;u*cVQ@|z6$EKPX!Y(t$=<#=b1xEr82PmTJY$Hlz%v<* zdyyZwRDiEWL88iLF_dvx-VBkYi`e&68MYQ#I%6Mx=H&b9#Ck5%i*=MN%{7>K=Ep7(tn8;6#6}Wp+khmS;|Y?ULP2BS?ZVgL!lCH z2o{q_Mmjx>48`(Ed5PZ>`sd6n?H4|wAIE))4V1jZ2TmwgNbRSwllHS%^*fA9d5KpP zy36IIekm{gr`LHT_W}!)_(!1r@Kfp|4JDKGKL^ITr|iptZUbfLA+W^)ox)!_0CZodqJ$dh^a3zVfkNqLD^-u*jU z<8H41QeNmu6i%0)#MkWE@+C_qKc$=iA4Floq<)DfDeYN~Cv){TEIj#nitDdT5PKoz zAJ5^`R!V7_tHRuG?HQuZ(BvXfd^M@2v|sEO!apW$$K@n#zh*B>641zZq+VgfYtE_V PU;KvUcq~^SmsS4mw)aWfKq=5u6tq6js6ktyrGXY(6>Vq&rx38xBB)>pX+nEVo0^WXQf}nuUeE+r2KIutl?)|*?e&6qY zzaN`l*?Xp|hT?yd zaK11Aa2mr=4b`bh%lD?Ju$HIu`c&l9E}IJds&~oa7Qsw7z^?#fQN~QSs|L5uyX7c`$%|j{gK+Ade!dPGJf(eCVebsrs+WN+( zyr$v_c@qlrgZ}(+tYd0s!XMexlo_)GOiLHxxEh6&W);@>QQdG}PxQ}ZU3J@s&)sD` z?Dbx-;@)SjZM8j-M>I5UYC|-Xh##Nw7(5XrqEYqnyl{bWxQTG3__N|KXXNs8yFGuY z8&x-??B+WkSsG|_oHJt8kTO&yO;@Ay)B343U;zE$ze_@Y8wS=NeHgs^!~Z!6esL21 z*CxTwPr~QHB>1~)hc{fSF*Ce6OPcn}`C&BMd zG9FJ7d>~1`UnHUbRTBK&kZ%;D*7qM9T<1;kL zcwR}u=dL7pR}%goC!t@Gg#Ncl=>IJV-jM{qFNs|5CZWGI34Lagajs4>U-Odi|4kA; zPUY&hq+NS)js<#=5Eg zB|*8?eRGf@GzhGM9)_9Xk?nziKfr(rzt3l1&98e1)()hozv~}yKf#}?_X?R#EAUFqP9Nh3CQKu zcKggqe-&{dVT;`Ik_HdND6aN97uQyM>g`;JLQRO;`b92ZZMD-CSlCzxEyn~YuJs3< z(2=K*rp{*Jz5QYZS_&|{fTzLda(nD`b>L63Rmz2KD2|mZ6SToBa8*0quAr<6mLR?C zaaAvgm)^Ossma;k2?YK15JFz!TvQO_=*P@@Ty>ge`k7#;O16ww&45#vS2?Q+oc78I z&WdT354h>aSmS6?!c~ zuQWgPi~01&&q3LLdJf1!+|Y*XvCpj2)bDSUnX>u}8W^YF>kPQ+7kbo@VDKf{N)y|ooAj}8u!rkO@dTVJqZY8h*t4sDe-8DB0=)YQ69H^B& zg4bOy`-D1AU7ddsQ6UPal}J9O&Q)74)Vu2aL7&IdAke~a`2^X$&?CEK83x08fFwNSvqY0cqU|p=V z&<$A$vJl>_;-@&uCzUzJ<&VoR5~h@uImd%Z*_=7nN##yU{&;~^&zS>sVZOx@2bSpQ z@=M~X#>G=w;%Ze&bXgcHS^A3ObwyCn=_C~=Pa{qw1CXa7H?aRxIZT*T_D6IBQA+1j zslX3HIRk%#g(3Jqjq{?CS|%=oQ67qRnJA0Mi9hk8{|Ukt#queL)wF%S@$A|`0;p^0 zfX3^u)zw#AykxKdj*YSUfkH8_ZwCE4f4mvcP~motbDekyJL(Xjox`C!a{&()9^&wl zJ%0qhLBdlUzJ<{b5MJPLErX5177oj~zrq%tDs%&;WPk>pypG`EwY5V9Xy#*%yZ^oi zaokH6zTogW1`iaz=CCSn8k3iFAfH9@rwBiyo|1O|pv4VYRu#XS-!Eu*p;X0pC*Zp| zK0P7+(BQ6>YCAfU@~aev*sa6!s~Lvx*5OC1P1*l@b$E)Mw6sr$FVc|66&=1rhd->t zU#`QS(BZGt;j_716ef{XYe|c@NR4#H){+(v5q^?}M6ShKguhycH?2?simP}i#t=C= zJPaz93Uqj~Z!Hz;@CfB%sZ@vOQ_kun9bUaJP@7cf@LH@#HFI?Mm`wmbUx$Zb#Zrw9 zpAo|fLW2&Eg%L|lI{c6rRuGzXco=>xwd?SiF{~hT>hPitze-4T*fW4$nhvR@<$^pQ)qYtHYnA!|&7K&(`4;9sV30 z{;&?O-Jw$T2_1f(&0@ye6|jMfevrd;V;zTb9DHNboc@t z-mJqH>+qv=_);DIVjW)6;V;qQD|GlA9e$1u&yPr~cD@dOsgAxzhtJdD8+7=59llA2 zAFIPR>+l6Se7g>B(cwFF_;EV?Djj~j4!>H5pP<99(cv%C;n(W$#X9^39sUX({#5al z22N?|;FJbVY2g1K4ICBE`(A44O_w^1ANU19YU`3yqTN!<_VjH`h-lG)Izc!R z9o2(OJYOLC8mc)G`7|1hb~2a_G)E$PVlW+KjzqS^U^>7Yi98>J>ELoC@X!E`V=5^=>~I*=TRTpNSwAaW#PjlpyPITEqNU^;jliChqa z>A-O$GAssXGT0DrdV=x8qkUs`f;12m? zFa_+8KL%5v4*6p+1?Z4J2Ga!tkpFFa_?AKL%644*6s7SO$Onqb5HE=$QW) zOo2J%kHHj>L;e^{fjH#XV5}y)k5-dSVCxC+1OE~ahZEqo1o-v@xG@2)OMpEI@Vo?g zRsuXd0k$W=6BFQ~1UN4N9+d!(On^ruz(W&YV*>nBZM?p|O@I$3z@I0;pCrKVCBSbb zz&jG)R}k0-znB*5VWxGe#`Jppb^fa?-qPXatI0iKlrPfvjD3Gl=O zxF`Y6OMpiuz#|jj5ee|n1lX7W|Aak=tsg0L$PxNVc>uo35&BYjj(oP%vD9pmI%IQ! zl>4*P7L|=kBkDn_gsJ{D^4CP|BrJ#vh`2OHEOFxbAeK5dnVYe{N#Ry=C*>u(f%)0dMXAG)mowo z{X^N!d!&m*;z`eBV>}r^)OR!LLrTV_sD+U86#=D)zki{`)?!wn6O8<4wh^ud1bvv{AHm zNugHr2H-7S%5-o-)ns44=6#h$`Rs(Q z?n?xmXup-$?bg@zsCEB~*FCGRTZzCF?N;%+PF5$j?L`yT=L`g@sBhu*4ZMDX+U!=2 zz8+{$E#=@;PIXjmT+h+tRjL{enmHBk;(CrAs#4A5;LpcNO0n$#JRHrt!5aEF@-!5S z$!VpzfL@oHvjnki7GhFpGx8j)SUkTJ6a6Hc0n6LWqkf?Tzo)rrRc8`b2$#oZ2q@;< zI;zZBLf&Qj?i%>Lsj{09Q*;|Y-FoCT=WvhO#ulF;}C96!P{oeLBpfs4PDAX z);MoHu3FjBhW>Qzf6v6+(rq=@uv~RdyIL6yHXSg*W+nkqfO<+hRb~QEWI8Gh_Y%O7^>8xcC@94^kF?v& z{#Fcr4A$mG_)i-AC|r!wNZV;#H5!Sc|q_T}bPgn>h> zPzrG-Bl6ZtYz`JOCP45N(%y9MnK)wJm5&{8vgjM1=O=XyKIBcy-BY^${b zhIW@4Zlyw#jsFzax0;uu3!12lP z7c>3wC`9{^>hfd$JZb;?v#1N+ZO6o(c~H^@Oe>8hT=XH%sLD$CEo~H75H@CvG|G}) zjmsjpprcNZZUrfm|7%tLTj3m-{FgC*t@(Fu4t?^o{wLmye&gk*p1;-gto6#|A4BrD z4Zs?XeEsjTPmLFENW6GISkTE@ct@Rh4y_~B#qY31MOIK3;iAD@!p((pWD-6> zUNfnNn`vowObpFUknnNp{PbiJ&Sl-3se4iF{x;B)f=hY#*I|}e_dk+rPTIXe?LH&1 z`}3&#`*pHwax(>+S@%y-uuky{Le+<%x{Y)t0!@s9AE%+TVXl>Kj0et};1Phyc@Krw zt(#yXOtJe|qhyB~1GRyTcX6t2%C(~p186t@h&sU@PXTeYG+K?2#kRH3fpph)3ZHGE zeK*dt&b7|7-eh%d!yqRWsDsQpq)yk%&?VE?tITmHGsr!-=EHP-ctgT;eKi*Ps=k;i zH^rUxn7#^_z8WdwpO5L_!t3nE$XGqiuLH$X=sa10ZhRt4ts`RfTQh=6%X2AmH4);O+CQP%rW;%}1@` zrQ5~X#&(Pch*4uuA?ExI4u6N3vsO_S&_S40=;#fHLtPz_5FUAIwHDU*AGWX9Y z7k$UdH~j^!e~;S7rQ7;=oTKs(@A(nIo;Ws*jvgZyX^ciL)iGL&@+!0>PH%$KM@Pj= z_X7Sz%!!Cqd3#{HV$L?P%D9V4WA};o(5G6&vFEbm%!L}yi8(6I?bK5mD{t*Fg3nH| z>MgL|CFUq#cQ<45iFnTd#$=3HT=pYWM$*xkL5Kqo^Z>K3h?@1$wJs z;PnI|&h(0RZ1Y;hJGPUtq{BqMhlwbTUe$)jqZ6-KPop2iIySzBZ1D)RsNW9cHy{?r z8lMJo(G|p@!g5Sm$xIKi0A1T>AehDqy$!g{@MnUu1=rqMUk_4WtFfk(S9-_<)e$t2 zk%qP@yMP0SLWts^%{mJaE9*mf99O>*;uY4z@VI*Pbth`Y_3PBICG0TEw1PU(w-l~2 zH#ow}&8=i(h(nr@BRFV4M!C%MCY~>4xk|>1)RGt5`-;nXQF(WSZeSOS>l<*wES4~a zrPOy5^-Z+YH#JwD1iiBP0JHM9nH+_*H=;_)-P=rOA`G1_!&w&Ns(1pL(6@P?m zV`9#A(ADo?6JF6j>P=jpYllNIS3M;5oE|$*DmS7{OL1Bw4ufCNomvOMC!+Ef9mch@ zSTNa^O>Q|{E|HBTo>;sF@$@Wiy#IgdADX(_jIQ7r6+Hw3WOb zl!^dZ>Gb~(m3l7S(%>La1HS?rphc}rL==r;UlrU7oHWkCIgDbAa*j$d>S{JnRE*(n zDZmG_>}B+;xutm;@<|}oKJfpD?+IOHyqeW^i_4a2wPS$I)vygZ0e;GX08#WExq;YY z`XejUopkFc%yM|*(JBzEfP|6fSx}~Wu$|N_{YzDDliG_a93&#S#hiB(DNPfah8-s^ zRp+YJqo!kAOcmS%VyJh3Jx#!}9n2D-LK2`x!~PLiW{;Y=Ds_1D7W0>JR$Pf1)i39= zQ)0}D-#A0pq|b_9WN1~&ZUl46es!2*U<0^r!x-t(Ei#6NfIA8b_dDY1+x+l}vhW?S~y@Z@yO#+zsf{H9!uOiu1V-+*W5%;K7#$U5s zl{_o2PK9vFC7w5Gby#U?9XLBuprR^v0$ob$av0MbE`lQGVJPByQ1qS5wG*}!R!`cv zW^Y#y+sZRTNwss$*^w0#kA)}hF+t?}p+3b4!vkZRxghc`w5N>yf^I#INB)6Lojq6& zZHq)14IZ+iJO$>f;hS{jhjcX0>&h)Unoo52M|AjCb>)4!vO!lqt}CAnujuVNKhkjM zzWS+|%z%_J5CQZ2$u$IiHBSnU*pFi5^UrvEN%WS3<)J+{Num8x%V&oxs_d37I#N#j z8ofkE1VR2yAAjIqCSGCV3zU_dkVoKE122^kaSV#?@b9`;^B6Qt0pW zuMF?Pah9DsLBOgC;*e!y&W_SuWIsyy_~EGhJp6gp1trLgu=$E*B?8}Mw5AX`0%ZtEK_S@ByvW+=Or zS%?(+g09g2p^V4iq)=*k2=P9gYN!kW(wB}9bo!NY4-EiVN5@4yoyvCvg(u#Mz+IV$ zvTk*HJO8iG*Q5W4pRYFw{&(h!=2tgDdfLTa9~vlilq!ynCkGye9;UZ_CeN`VtS*My zf09R9#n)tW8J(8T1$&yxd5F|7g%kIw&uvG6YdJLXS_aKjc!ZstZbvB&-`(Fmi{=-Wz~=a$}rI8(^6bYVw0Yk(tMZO$%jXP3^h z`8+QI_Omx)u}8glMs+UCt5aNWuxt%=d-o`zUEaHn8&dCD!&UB?w0(}AG7=nMZ&nmv);Xe@0#*mwag zx15;uif!$r*p}@D+EvOn#envRavfpWY{F7d{RL+@EnIl9BX_@Ykfy7xt2{hB%^_a1 zUwI3#BUh0oZXTDBdgr4!AQ*3@BVy=tw)_WoD$_oR+0$1v;kOY?_Eg^TnK>-eIN_f&JwQNn7LOaB^nIIHjvYCh2+HV(j+6`IWEnVrhcFzEB$et3~OAtzF zQrkyj>wQqI)G-*YmF#SMmON|A7l)5+6UEk# z5#mYVvFqw_K$bgZP29WcZu-k_h%9We#H&+8SBg z(WspozLJJ;no2TEB{>Zw0~kpxmY?b6JmE>%YWH6F8&SJP)e~wyzF6sxPeo-wA?+mqW zq*`|#>V{C=bgIi%>&{o}OsE@5br$MYYT3z0et}wjp;U5o5r*$bIahpXSi5Yzh_Wi9 z8F5KDR_8$GqAah|b6Ui$LxumD9_pU?SlZT(tHkv?nU|W>UNvgqq;}h=bYZOam+CwdfrmY~nnwVQvte z!*N|gj;j!4ddD@K;L+HKRnK)V7FS*u9oE8g%~@iu&z#khN%~bf_Qw1di(g(LIOe}r zMW#bYEHVSG-%OB$yiMehAEHr97hCT*c}+63JM06+mxfG2j<}@@i*H-w7wr8@?DJdk z@`JJiTonN#Q+x?e7NPM{{{*v;Wh2QR`p&um{h`AvFXQpt`e8P0`geNoI>tAso$3bl zKEu!k^}f16{r8jL>dI@~qu9{lT}KS7*BES~eXF`j{Z(w6hr!xHUE9=QQ%M&qX!40U z-D1^<3vo>&j(uaX9oL#?qr8YNH%2ZcY|cLs%Hqas?gwHHuE5xB%63HPHgB92XbX+Oo1mEueAn7wo2wiY<+jhQBLh(5E4HbI*s zywrUDmVH^)LEA%z#r2DErFb>KLEDrEar3{K-ilQ=;kYcWZ$#DY!pj4JVLX(^7{Q}@ z0=j|QI24^IRWX8FLEz}HW6%E15jwV!_H%4vIAGcwAv~T{9?66TSPwY-;~j~O&D8$g z6J%>-^LU?aC_O`BCwS@<|VMTg{sXqWeR5rZU{{`-c1Z))BQ1Y#2CJf zQ#(mXV|h6pqf{mBz-6AQisu=D%`C;DjK*`YTIXOjv%9#yRU4sCEi1#onwfWKN920g zzgC`(a%U`_n@ux`m>_h_I;%YN6XHvX3~Hp%aI=J&s$jlQLb!$vK0+EpT3(VQk zK_dTM%n@Qf`66jql;$OV1h(b;0jg4vND{4azYf z@j3_ev5|n4K$He#%<3N^1FOn0#97P0aToNTe9f!+v?8xly5`gCVtX zAGT6+!E3ajm9ilim$<&loQnT#<}|IT)ZsH1Y}ubas2hQtN?~La12kj`YldhD&C)d_ zf*bHc@B?IqhD1CAB;p+)hiFKIQ$Qlb0WwoVW+)e7#NtbKM3=KtNSRUxF8;||bOSRW zWI$aS;j&fS=2__>18o+#^xrCOwrAkp$UyA?qb)`T1j0}KEH<7;>%5ev5o_v3Dfg?` zW6;QxP^YpP|7okJ5YwWS&p}yte-S#2xRRewiG+?w9apzSn+7Y-L5f|eY>Vl*4S`}= zHa(#|P3j0{--tNH5V;avkl((MSTO&4-uHCUvWDJ;XXU4@UD)Lky(?aBJiJHsAg`qi z!mVMK((raP8cM~bWBVDf&d@H)+pi&hrkXE6Sswbl;|hAC>^qQI@A^@A5PqLgZ^nJFJe&xo0hy8|2pizuaLuWNzJeoFo&hhl)Coo%X=c2smS#DIsTk66 z#i-%9ZfM#6Gp>faNa-z~eumg1$cGwz^Ii!NADcE77h={-RuSxHE(#~Y0l8D zIh6IpSv`v>2T*2Y5;`JZIGnxYDYkutQIS7Xmmt@lA0QkxLXQllw&?lYD(_23h+kOv= zhBKL$KE1<3yEW4lIhS))sm~Ee19%+=f|u&|r~kQfUujka{|5}s+Hs{hCi#lDqS4lm zWZd;>dsJVrAhs1j6MYMWz1^Ysz=H0VcSvD8GpDDj>1_U)i;VHc#w3LcbCelm+4{{X z4~V||i50XteF*QM+nn%FhWwcFyFbV8Pwx=S_osn(bVjbhDpCH9nZsd&hJh1o-+)S) zr%sPea_;`A;X;$LnI>9yTk>0iM^jsN>JV3^VclR%#>stSvSGs<-nW93!3pEKLK_#m zKWS&5s#11BuR4FM-JwvZOV=m}jz?U9F5;FJ?!Y!k-@wz1?OuEuV(#-muwlbsc+O={ zk&_*}XCqZp-Ov=&ZB*;%t+g%tag^S>nVhu)4rtIj$_NZ#Gdbi?Hs*?~SxEtkT+*&m zv_SI|knbvm=ZE?ccZM4Ww`Xt2GSTgo-GB=NJMLpr+tnMGSw^-BP(zm%_7r|EGYI$H z|18` zJ`N^1@esSJimB z5h=$lV%stKeA>lvn&7*5V-BVzr*As8*+S!cK4wqdl}||udXq+(4V@_C0IH(zYPmva zOa-NKUV?z?`JT&HPDh+L`QlG8cz&V$w2L(L1(OQW>!re7Mu{RjI!o7egrz zrJ38_mUEcDjGYG>^B@L)c)|Q^Z2z}~M)$l!Z)Kd$Cg`}*0c9#)2ox{NXk{h{)csxB z01Kpar*#%Pt>Fob`81r<*v)&ov}+2Y7I>6l1ItUcFTRO}2Fw1E9=&`5?lZz;On5Rv zSG5$U-3&X~rO2p2nLXy=0TZ(ayXBy?i_Y0N$*F7;sBojwfMd22gr>OVNga4YDwq}f zJXaIh2h6QglZqY*T|t}A233ye)!H?5dFV%y<0yQ^34i+H(TevEs6)X>cW9i+BGESUO4|k|K#^I~@i^yn7n;LXX)qQ78{Z?3QD8 zOVnZb2@l;uAJU0MYB^w#O;Y&w6v^;5y{(6ntpk&&Y~2ZsAS!bpa!sG!_PV&T6CF(_ zW-p=OF!XrK#rj>-u_Us}!)Z=>F1MQ^RI4})wvlAMEw6h+xaysXTgV=Rj7!V3>!r|N z@#+}l3D3dP)(@3(bWBnjJ}9TbByi$>L3>{6pqKmK!0R?_%;!PVnywB<5W;cT0!cl853Ynx5IzrU;Z_pM_OtnKVw7B=;~ zxDNPvrW+jAvf=_ukF^!Jl1>flLy-#%MeVaJX7z# zF7JH=^=t1x*u2hk;Q6&Z|MsjYrz(JNbTrMbt(I$YFE^=DpnsxY-FooR)TO?jaAk-9bn@qj0tT}k^hMp&$?D_4pIeI3!2WUPYT6^GywLP7y zdVde#fqQ!&Sk-&)pL#mldpe%X*W`(fL*E=t*I^8pUmCy#M0+C{$&2genP4_2ch37V zL01P}{1c5hBd&p^pg_8nBjL~3Zrk>xe!7TPXh53XP{pP-d4?N9>X3FXP*S4PD zEx+6}$<*^g>%n_BnR-@r_S}2FsSMvUI=U=30|Od*&W+P<9F zaJD=iz_4LUs~;dm_N-lL>e;ZoXLUQK?BHF`!lYIo*wAk3ePI=ud+(F?<>y1YapJ3s z?6KYl*O^G>CmuWS(uSVjtN{bAvEJ2p^**)<6<9+gI!)R`?fTh@%@gVW;L2w)@A0z) zrK=8vd7o;J*VX8govsFaKWnuxw%W63tSm2?Rf%{1^YR+}0hwnOo{vLbTTM`H^*DT? ztG^aSEDL-YE53V~Pq>AwE%* z6^{{`6JQ3Ko?t_ zQ>8-l6+7@}2fVrW+G5}f%LL)@r0Qn}Ty^j_o37d7fOMgc@HECBv;V1w&Jo;hZ4@(>uYN@OUvu zXvag6i;2QEkOkLf6btb;A0cc7`Vum;CTC_(6$dX)Zx*gT`|?Z2n=b^GwV#W;{T0YW z2%dPR<-VVgr>8)=WiV9_PV9hLU8EDIFNIf$xBeNJN^am1M zeMh^v?t2hX-GXgC5fP%-+au=H@(s57Vu5G6HFP;{&Z*4boVkQ zs<`|l-Ps3|ZFho274$L>WlA*W?dW6vp=k7Nl+%i|KJ4-1hPf{r!26h=vN+kOi680i zF!;Sj{Eh-(V`ltd<9xpL{3=pz>X+Zt!9oRiRD;EQyrbMj{GJC;LGuE)pkFgnupF%k zYi9D`8xk;Bsc4SB4Pf&u{NYQGJ?&-y%qIKhirJ(+Ci9d`%#}BpV!_XZzadcF+>OyF zUL1oS!4r-2hDmmtg*8wGS>8v#!u9>E1Ev?Y4yI&Qq-;r|mqq=p4r$xYKfm>tze<@E z`d;DB(bXD^Z7IEsh&=5Hbk5|34p)%h38$T8o2w0}#UyWki!gN))nKGp5&Go`r(!~1 z!?Cnv?0sWUPMeIrGFQjxf%IPtn#bUy>32HPE(Hy<5%`{ye1b%FJtea_E%oZbYOg0b z1@JTBZ#j78px^VL`m_hB-{v&ECbB`(iGAraJlh$KGOhn*DK}u{t0@{aWKG5V93A+A zp*cf1HV9?l(lXVY%4aM>Ib<;Y+9*7d`jO#Yqp-=S)@EGuFL*oPEdz(_^Pe#YFKf_M z&c_Y-t`fEG>{+)6!ru(WKcosL3>RQ(QjEV%6<$xdR_*O-ieVuNEe7MKV}!j%rXR2o+q4Ol^+^md^%5f zf5b?ze&$TV>^jr<{21YP*%`oW&bFbir_OqIC^|cMF%4!#&U}==%q7y+OO3CN5k4Jl z1kv%)R0|ave@%Kgl}>5klm<>|;FJbVY2cIwPHEtj22N?|K+}NsJ5EBWis|M# zPIWBNVTh6j({Ehq;VB6){YI9)o_>>x(hxOM50EI++booJ-=yYv6`v*Tcc#waFx{n4 z!bdGw(tcM;yARXOl;8dw^%KEboVzLUZiQn0pod?R3Ws^U_WUQESI~V0B|5uO!W+qP zi8m9{SEy(leOcOvvoNLMYDU+~EYVkPQ>vlMGb9bK3{zok+|69E>^&z`Y5miO1t@8B zKSWjeK+aHjj?LuV%@u@ueg^a5&{zL3xRlp_mr$>A|L^->`+f05js+$aQ_AxSp3mob z1J9dz-pTXTJYUQ6E}nPud>_vb^IW(=DGGGs#zDR<1q2Ofaj$=ui*K7o;UEkndhB6U(NHiJn!OpH_!L+{4mdj zi@5weH}SlH=cPQa;Q4%>tF8X)|7*YFW=LR#pB%j0lrw8VW4+vHvRLpdg9Uk(Mh05$ z7&k7zpfG=YF0V*v8V3^JG~sK!sS%%O-O%s(El*YPCSJ|%toxzA6!mm-U0_uSwPz34 za{OVAXZQZVZQ%IMNh*DTnlXn+e7ZQEJ!3%KObnFptIJeEjn8u&UuskFO`Ojmj_=}l z_IvCfVnkTD-bf-^gD#v$Stis&A2+KIWn=Xct=)RTGa~%IJr>8qpN*{21=L{95 z`$|d~$Z7oFaC*91q(skn2yePXh3W2((o~KYN*O4y`#u!iz#H(pzt8_l#q;3^0pRkH^?C|@li+Vlf?p0i$=fws<-3KiuQd$MJJV&` zlJI#y3I0olKlyjNdl`S|Ibq&S2w3{l&(tLN-zLGo0Q|6&vjuH`&_vjrgnmyFd?X3} zcoO_5L`nUvixS}bl?(U_Fi!1vJXw&1!FrR>Hz&bAP56{yvA9-~_m4^FyOQ8PNrFF+ z1pg!OByZyHub+txvOjqXlHf~{;Fn`voJQ-hRF&vrF4*)W^!Fvf|1JrB1MqCVxE-?R zcIab&68i6x;71^^?@!**gipaK#KZ^AGYjx!pNam5UQ_Fj&%;UZuP4EOoCJR;3I24Z z=lC%alnXrh)n@LOviOF2brSmPv40JNAIswOn!gGrpq$qfpV;O|L-e;9aLFP%52yx!(4KLFlzGJ8Quf~QaF&^Q|$ zY9sA^g)@LBd((c8ShKe=jQ->fu1bOzD%}CulJCdwp1T&*I%U^Fp$V^UI_vy+*Oy)r zmIHoYzPq6TzZic*A^k?Xea5s|KD_qjwL9HDI~DTTyTlCve*=EI+eM-(;~^#_3;gE_c%a+%>L%lir(^gXL~}r3?SrC%A(2v+{QQ z#QlGXPuaNg%JCQ&_{^M!l74{yR}>Hvx>7F0`;xx?M6}O@*yB}2`0;AD$5qD_SUJI| z6W3Bu=?T=jd=+@fvUqWzR`$&FEVS3iauS*njV1^ph$~M}-{UNop;Q^O6C2zNJ?sC& zW>B54nsjEdOed4G1!@<00+xc>`dV2n+v7*}ACo0vIgV^`vd2@s056T&XL>6OoHJ`H zEpe)eWBwmBP`jZO!8@GdxMq^o;dBNY7dXqE3!Q<+`uf`Xg%GY%b`(3EwSE#xz1u2e-2S=-pGWpolkN9M z;q-#L(-jD~mN-3FwMztVz*XmQRyWqwEkP3<g~*?pZZ4hT zD4$g39G5>nzmP2u|M-SR+3r^HbLKcTe4=rcS?sXt2{d4sEIw%vaQdJy*c~jffQIS` z1nT`xpWjVxr9Rftxcnm4k>*s!JFgpO85djvWiw5?ZysOoUu<8bA3?sQ1g{QP`y1sr zHh2Txd2J(R&grRk$*x3Ui~V?onHCd<1Sj@OeG-0o3);i7`lSsoBa?6iJmiJqq!?!* zjwMd#!um$?j}u^$vfowi^}46R*TSV%Os;ZPOslduD=Of_yQ;hPc+y3m<8k#(LrC?QTfZ$@LCHl;zqHakDy@#e$qnwp&W)JxD$b0sfvE>eR6f05VM7_5n3a%>#oQxFKO@~JSc|W#b*gT^>#NmH<#>bs*Tq^iCs=^TyJ9N!`f5}*A1=ug5@WNAN}pk z?kcC9R$Tn*1&{j0b?PW%ezDqDv8b-JY|b3(q;jVPPP7DMC)={+GFVgmCoAlA0y7Bz z$p%4Qfj&L~_w4k#YWu9HP>|gVJ+e!d12JtecS>FunyH#zrfn3E*8=y#!wu{!K~F&b z#lcAh#ZbI|Rc2K{{p482LpC-$Z1;+{qI`=*@G|R)8!*Lsu0FZcEkPZzYHkZ$)lT}< zN8By|>9K-NmL9Sex?_7oY;I^8)VP&zF*3r8CA66*-(mvzd`jF%v8J)Js8)<^2A_z5 zQ-!KG;@B29BLK zUM-B~m-;tL0j4eBY4Ew+9(!FK#uaDV`RZO(=_fzPt*6?K)#9&;cZDUieCzy+Jn;(X z$JgkV2^L9-+vRhY%`CG_sF+q$CpgQiXoYA|7c7}#t}538xKig0mhq~^NX)keurbf7 zUtC*X?VRZe!Y`KMSqTCae#H5unji_)PgB}w)}5q;;4J|;!6f?I!hN;wskQ%yd$@C9 zoxh$tMr`RMc~ZIYF)ZH_8|>>=sDpM}X6)GJRCRM>hfN0)CV;$9yxrIm?MBECE~%4U z3y{O6Sze=+5FdI14MILXYU;^HxROtYNVt$CI@E&3T3@B3;6@W?Y&8NZ-DO zx7TuW95p>_icjSU?X~BIT5gIbKdC)k&*NiPeocPueFH7OlDEgTI!lS;cL9s5t(g4U z`v_W2&lxC@jHG9+z4l%}EpQ~V*1z_CgO+RWGk}C8;-OEs0HAjTwDzT0YOR(ps#wz%TvLS z?vk}>)}Ci~^Y$7YV}}1USj$DUClR&w+H>Z;ynO*zEUjNkT03nX{s0`wq_x-Hhe{{M zfJC2<)hDgJmS>}0-#+%dp;>KZ(llLu0~e{g-yCHKt}Yr_UqmQ>h+6`6Awb zK>}WDpO=8Au~O3K30<)Iie{Dx-Q?Vn=>4q=@u!Vn^Iv#u6&pK>v1D4V78j6l=#tj2 Xh8l)~CcB2C{O(6oXiP$b1W@?joqh#v