robin
ed34e1c025
Previously, once a wheel's airborne flag cleared, its target reverted to the normal drive command but the actual RPM still had to ramp up through the gentle standstill-launch accel_rate/jerk profile. Since the chassis is already moving at that point (unlike a stop-start), the wheel spent several hundred ms unable to match ground speed, dragging/grinding against the surface and occasionally landing on a momentarily negative curve-turn target, making it spin backward. Track the airborne->grounded transition per wheel and, on the tick it clears, snap cmd directly to target and reset the jerk accel state instead of ramping, so the wheel rejoins the other three immediately. Verified working on hardware. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
1382 lines
54 KiB
C++
1382 lines
54 KiB
C++
#include <SDL2/SDL.h>
|
|
#include <algorithm>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <csignal>
|
|
#include <fcntl.h>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <sys/ioctl.h>
|
|
#include <termios.h>
|
|
#include <thread>
|
|
#include <unistd.h>
|
|
#include <vector>
|
|
|
|
#include "livox_lidar_api.h"
|
|
#include "livox_lidar_def.h"
|
|
|
|
// Global flag for signal handling
|
|
volatile std::sig_atomic_t g_running = 1;
|
|
|
|
void signalHandler(int signum) {
|
|
(void)signum;
|
|
g_running = 0;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
class SerialPort {
|
|
private:
|
|
int fd_;
|
|
std::string port_name_;
|
|
|
|
public:
|
|
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;
|
|
}
|
|
|
|
void closePort() {
|
|
if (fd_ >= 0) {
|
|
close(fd_);
|
|
fd_ = -1;
|
|
}
|
|
}
|
|
|
|
bool writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
|
|
uint8_t pkt[8];
|
|
pkt[0] = slave;
|
|
pkt[1] = 0x06;
|
|
pkt[2] = (reg >> 8) & 0xFF;
|
|
pkt[3] = reg & 0xFF;
|
|
pkt[4] = (val >> 8) & 0xFF;
|
|
pkt[5] = val & 0xFF;
|
|
|
|
uint16_t crc = calcCRC16(pkt, 6);
|
|
pkt[6] = crc & 0xFF;
|
|
pkt[7] = (crc >> 8) & 0xFF;
|
|
|
|
tcflush(fd_, TCIOFLUSH);
|
|
ssize_t written = write(fd_, pkt, 8);
|
|
if (written != 8)
|
|
return false;
|
|
|
|
if (slave == 0)
|
|
return true; // Broadcast
|
|
|
|
uint8_t rx_buf[8];
|
|
ssize_t rx_bytes = 0;
|
|
auto start = std::chrono::steady_clock::now();
|
|
while (rx_bytes < 8) {
|
|
ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
|
|
if (res > 0)
|
|
rx_bytes += res;
|
|
auto now = std::chrono::steady_clock::now();
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
|
|
.count() > 50)
|
|
break;
|
|
std::this_thread::sleep_for(std::chrono::microseconds(500));
|
|
}
|
|
if (rx_bytes != 8)
|
|
return false;
|
|
|
|
// CRC 검증: 손상된 응답을 정상으로 오인하지 않도록 확인
|
|
uint16_t resp_crc = calcCRC16(rx_buf, 6);
|
|
uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) |
|
|
(static_cast<uint16_t>(rx_buf[7]) << 8);
|
|
return resp_crc == recv_crc && rx_buf[0] == slave;
|
|
}
|
|
|
|
bool writeRegs(uint8_t slave, uint16_t reg,
|
|
const std::vector<uint16_t> &vals) {
|
|
size_t count = vals.size();
|
|
size_t pkt_len = 7 + count * 2 + 2;
|
|
std::vector<uint8_t> pkt(pkt_len);
|
|
|
|
pkt[0] = slave;
|
|
pkt[1] = 0x10;
|
|
pkt[2] = (reg >> 8) & 0xFF;
|
|
pkt[3] = reg & 0xFF;
|
|
pkt[4] = (count >> 8) & 0xFF;
|
|
pkt[5] = count & 0xFF;
|
|
pkt[6] = static_cast<uint8_t>(count * 2);
|
|
|
|
for (size_t i = 0; i < count; ++i) {
|
|
pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
|
|
pkt[8 + i * 2] = vals[i] & 0xFF;
|
|
}
|
|
|
|
uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
|
|
pkt[pkt_len - 2] = crc & 0xFF;
|
|
pkt[pkt_len - 1] = (crc >> 8) & 0xFF;
|
|
|
|
tcflush(fd_, TCIOFLUSH);
|
|
ssize_t written = write(fd_, pkt.data(), pkt_len);
|
|
if (written != static_cast<ssize_t>(pkt_len))
|
|
return false;
|
|
|
|
if (slave == 0)
|
|
return true;
|
|
|
|
uint8_t rx_buf[8];
|
|
ssize_t rx_bytes = 0;
|
|
auto start = std::chrono::steady_clock::now();
|
|
while (rx_bytes < 8) {
|
|
ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
|
|
if (res > 0)
|
|
rx_bytes += res;
|
|
auto now = std::chrono::steady_clock::now();
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
|
|
.count() > 50)
|
|
break;
|
|
std::this_thread::sleep_for(std::chrono::microseconds(500));
|
|
}
|
|
if (rx_bytes != 8)
|
|
return false;
|
|
|
|
uint16_t resp_crc = calcCRC16(rx_buf, 6);
|
|
uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) |
|
|
(static_cast<uint16_t>(rx_buf[7]) << 8);
|
|
return resp_crc == recv_crc && rx_buf[0] == slave;
|
|
}
|
|
|
|
bool readRegs(uint8_t slave, uint16_t reg, uint16_t count,
|
|
std::vector<uint16_t> &out_vals) {
|
|
uint8_t pkt[8];
|
|
pkt[0] = slave;
|
|
pkt[1] = 0x03;
|
|
pkt[2] = (reg >> 8) & 0xFF;
|
|
pkt[3] = reg & 0xFF;
|
|
pkt[4] = (count >> 8) & 0xFF;
|
|
pkt[5] = count & 0xFF;
|
|
|
|
uint16_t crc = calcCRC16(pkt, 6);
|
|
pkt[6] = crc & 0xFF;
|
|
pkt[7] = (crc >> 8) & 0xFF;
|
|
|
|
tcflush(fd_, TCIOFLUSH);
|
|
if (write(fd_, pkt, 8) != 8)
|
|
return false;
|
|
|
|
size_t expected_bytes = 5 + count * 2;
|
|
std::vector<uint8_t> rx_buf(expected_bytes);
|
|
size_t rx_bytes = 0;
|
|
|
|
auto start = std::chrono::steady_clock::now();
|
|
while (rx_bytes < expected_bytes) {
|
|
ssize_t res =
|
|
read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
|
|
if (res > 0)
|
|
rx_bytes += res;
|
|
auto now = std::chrono::steady_clock::now();
|
|
// 15ms -> 25ms: 100Hz(10ms 주기) 루프 내에서 응답을 안정적으로 받기 위한
|
|
// 여유 확보
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start)
|
|
.count() > 25)
|
|
break;
|
|
std::this_thread::sleep_for(std::chrono::microseconds(100));
|
|
}
|
|
|
|
if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
|
|
// CRC 검증 추가: 노이즈로 손상된 응답을 정상 데이터로 오인하지 않도록
|
|
// 방지
|
|
uint16_t resp_crc = calcCRC16(rx_buf.data(), expected_bytes - 2);
|
|
uint16_t recv_crc =
|
|
static_cast<uint16_t>(rx_buf[expected_bytes - 2]) |
|
|
(static_cast<uint16_t>(rx_buf[expected_bytes - 1]) << 8);
|
|
if (resp_crc != recv_crc)
|
|
return false;
|
|
|
|
out_vals.resize(count);
|
|
for (uint16_t i = 0; i < count; ++i) {
|
|
out_vals[i] =
|
|
(static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
|
|
}
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
};
|
|
|
|
class MotorDriver {
|
|
private:
|
|
SerialPort *port_;
|
|
uint8_t slave_id_;
|
|
|
|
public:
|
|
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));
|
|
|
|
port_->writeRegs(slave_id_, 0x2080, {acl_ms, acl_ms}); // Accel
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
port_->writeRegs(slave_id_, 0x2082, {dcl_ms, dcl_ms}); // Decel
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
|
|
port_->writeReg(slave_id_, 0x200E, 0x0008); // Enable
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
setBrakes(true);
|
|
return true;
|
|
}
|
|
|
|
void setBrakes(bool lock) {
|
|
uint16_t val = lock ? 1 : 0;
|
|
port_->writeReg(slave_id_, 0x201A, val);
|
|
port_->writeReg(slave_id_, 0x201B, val);
|
|
}
|
|
|
|
void setRPMs(float l_rpm, float r_rpm) {
|
|
int16_t l_val = static_cast<int16_t>(std::clamp(l_rpm, -3000.0f, 3000.0f));
|
|
int16_t r_val = static_cast<int16_t>(std::clamp(r_rpm, -3000.0f, 3000.0f));
|
|
port_->writeRegs(
|
|
slave_id_, 0x2088,
|
|
{static_cast<uint16_t>(l_val), static_cast<uint16_t>(r_val)});
|
|
}
|
|
|
|
bool readFeedback(float &l_fb, float &r_fb, int32_t &l_tick,
|
|
int32_t &r_tick, float &l_torque_a, float &r_torque_a,
|
|
uint16_t &err_l, uint16_t &err_r) {
|
|
std::vector<uint16_t> regs;
|
|
// 0x20A5~0x20AE 10레지스터 연속 읽기: 에러코드(0x20A5/0x20A6)부터
|
|
// 포지션 틱, RPM 피드백, 실제 토크(전류)까지 한 번의 통신으로 확보.
|
|
// 무부하(공중에 뜬) 바퀴는 전류가 급격히 낮아지므로 슬립 진단에 사용,
|
|
// 에러코드는 과전류/과부하 등 드라이버 알람 발생 시 원인 진단에 사용.
|
|
if (port_->readRegs(slave_id_, 0x20A5, 10, regs)) {
|
|
err_l = regs[0];
|
|
err_r = regs[1];
|
|
|
|
// 수정: uint32_t -> int32_t 캐스팅은 2의 보수 표현에서 안전하게 부호가
|
|
// 재해석됨. 기존의 "val - 0x100000000ULL" 방식은 uint64_t 승격 후
|
|
// int32_t로 축소 캐스팅하는 구현정의 동작(현실적으로는 대부분 동작하지만
|
|
// 명확성/이식성이 떨어짐)이라 제거함.
|
|
uint32_t val_l = (static_cast<uint32_t>(regs[2]) << 16) | regs[3];
|
|
l_tick = static_cast<int32_t>(val_l);
|
|
|
|
uint32_t val_r = (static_cast<uint32_t>(regs[4]) << 16) | regs[5];
|
|
r_tick = static_cast<int32_t>(val_r);
|
|
|
|
int16_t vl = static_cast<int16_t>(regs[6]);
|
|
int16_t vr = static_cast<int16_t>(regs[7]);
|
|
l_fb = static_cast<float>(vl) * 0.1f; // 0.1RPM 단위 -> RPM
|
|
r_fb = static_cast<float>(vr) * 0.1f;
|
|
|
|
int16_t tl = static_cast<int16_t>(regs[8]);
|
|
int16_t tr = static_cast<int16_t>(regs[9]);
|
|
l_torque_a = static_cast<float>(tl) * 0.1f; // 0.1A 단위 -> A
|
|
r_torque_a = static_cast<float>(tr) * 0.1f;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// 정격전류(0x2033/0x2063)·최대전류(0x2034/0x2064) 실측 조회. 채널당
|
|
// 2레지스터씩 떨어져 있어 L/R 두 번 읽음(설정값이라 시작 시 1회만 조회).
|
|
bool readCurrentLimits(uint16_t &rated_l, uint16_t &max_l, uint16_t &rated_r,
|
|
uint16_t &max_r) {
|
|
std::vector<uint16_t> regs_l, regs_r;
|
|
if (!port_->readRegs(slave_id_, 0x2033, 2, regs_l))
|
|
return false;
|
|
if (!port_->readRegs(slave_id_, 0x2063, 2, regs_r))
|
|
return false;
|
|
rated_l = regs_l[0];
|
|
max_l = regs_l[1];
|
|
rated_r = regs_r[0];
|
|
max_r = regs_r[1];
|
|
return true;
|
|
}
|
|
|
|
// 최대전류(0x2034/0x2064)를 좌/우 동일 값으로 설정. 단위 0.1A.
|
|
void setMaxCurrent(uint16_t max_current_01a) {
|
|
port_->writeReg(slave_id_, 0x2034, max_current_01a);
|
|
port_->writeReg(slave_id_, 0x2064, max_current_01a);
|
|
}
|
|
};
|
|
|
|
// 드라이버 에러코드(0x20A5/0x20A6) 비트마스크를 사람이 읽을 수 있는 설명으로 변환
|
|
std::string decodeDriverError(uint16_t code) {
|
|
if (code == 0)
|
|
return "";
|
|
std::string s;
|
|
auto add = [&](uint16_t bit, const char *name) {
|
|
if (code & bit) {
|
|
if (!s.empty())
|
|
s += "+";
|
|
s += name;
|
|
}
|
|
};
|
|
add(0x0001, "과전압");
|
|
add(0x0002, "저전압");
|
|
add(0x0004, "과전류");
|
|
add(0x0008, "과부하");
|
|
add(0x0010, "전류이상(예약)");
|
|
add(0x0020, "엔코더오차");
|
|
add(0x0040, "속도이상(예약)");
|
|
add(0x0080, "기준전압오류");
|
|
add(0x0100, "EEPROM오류");
|
|
add(0x0200, "홀센서오류");
|
|
add(0x0400, "모터과열");
|
|
add(0x0800, "엔코더오류");
|
|
add(0x2000, "속도설정오류");
|
|
if (s.empty()) {
|
|
char buf[32];
|
|
snprintf(buf, sizeof(buf), "알수없음(0x%04X)", code);
|
|
s = buf;
|
|
}
|
|
return s;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// 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;
|
|
};
|
|
|
|
class LidarObstacleDetector {
|
|
private:
|
|
std::string config_path_;
|
|
std::atomic<bool> initialized_{false};
|
|
std::mutex status_mutex_;
|
|
ObstacleStatus status_;
|
|
|
|
// 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)
|
|
|
|
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;
|
|
}
|
|
|
|
SetLivoxLidarPointCloudCallBack(PointCloudCallbackStatic, nullptr);
|
|
SetLivoxLidarInfoChangeCallback(InfoChangeCallbackStatic, nullptr);
|
|
|
|
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;
|
|
}
|
|
|
|
void stop() {
|
|
if (initialized_) {
|
|
LivoxLidarSdkUninit();
|
|
initialized_ = false;
|
|
std::cout << "[정보] Livox Mid-360S 라이다 수신기 종료.\n";
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void handlePointCloud(uint32_t handle, LivoxLidarEthernetPacket *packet) {
|
|
(void)handle;
|
|
if (!packet)
|
|
return;
|
|
|
|
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;
|
|
}
|
|
}
|
|
};
|
|
|
|
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::lock_guard<std::mutex> lock(status_mutex_);
|
|
auto updateEMA = [](float &old_val, float new_val) {
|
|
if (new_val < old_val) {
|
|
old_val = new_val; // Fast response to approaching obstacles
|
|
} else {
|
|
old_val = old_val * 0.70f + new_val * 0.30f; // Smooth recovery
|
|
}
|
|
};
|
|
|
|
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();
|
|
}
|
|
|
|
ObstacleStatus getStatus() {
|
|
std::lock_guard<std::mutex> lock(status_mutex_);
|
|
auto now = std::chrono::steady_clock::now();
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
now - status_.last_update)
|
|
.count() > 800) {
|
|
status_.connected = false;
|
|
}
|
|
return status_;
|
|
}
|
|
};
|
|
|
|
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 <파일경로>로 지정
|
|
// 실측 결과 정격15A/최대30A(공장 기본값) 그대로였음. 정격 위로 5A 여유는
|
|
// 남기되, 걸림/과부하 상황에서 30A까지 밀어붙이며 3초씩 버티다 과부하
|
|
// 알람이 터지는 걸 막기 위해 기본값을 20A로 낮춤. --max_current_a로 조정 가능.
|
|
// 음수를 주면 미변경(공장/기존 설정 유지).
|
|
float max_current_a = 20.0f;
|
|
|
|
// LiDAR Parameters & Robot Physical Specs (User Specification)
|
|
bool use_lidar = true;
|
|
std::string lidar_config = "mid360s_config.json";
|
|
float front_stop_dist = 0.35f; // m (전방 완전 정지 거리 0.35m)
|
|
float front_warn_dist = 0.50f; // m (전방 감속/경고 시작 거리 0.50m = 50cm)
|
|
float side_dodge_dist = 0.50f; // m (측면 장애물 우회 감지 거리 0.50m = 50cm)
|
|
float side_stop_dist = 0.20f; // m (측면 한계 접근 거리 0.20m)
|
|
float max_dodge_omega = 0.35f; // rad/s (자동 회피 최대 조향 각속도)
|
|
float robot_width = 0.410f; // m (로봇 가로/좌우 실측 전폭 410mm)
|
|
float robot_length = 0.631f; // m (로봇 세로/전후 실측 전장 631mm)
|
|
float robot_height = 0.286f; // m (로봇 전고 286mm)
|
|
float lidar_height = 0.460f; // m (지면 기준 라이다 높이 460mm)
|
|
float lidar_pitch = 17.0f; // deg (라이다 하향 기울임 17도)
|
|
float lidar_x_offset = 0.250f; // m (로봇 중심 기준 라이다 전방 오프셋 250mm)
|
|
float min_z = -lidar_height + 0.06f; // m (-0.40m: 지면 감지 방지 안전 오프셋)
|
|
float max_z = 0.80f; // m (라이다 기준 상단 높이)
|
|
|
|
for (int i = 1; i < argc; ++i) {
|
|
std::string arg = argv[i];
|
|
if (arg == "--port" && i + 1 < argc)
|
|
port1 = argv[++i];
|
|
else if (arg == "--port1" && i + 1 < argc)
|
|
port1 = argv[++i];
|
|
else if (arg == "--port2" && i + 1 < argc)
|
|
port2 = argv[++i];
|
|
else if (arg == "--id1" && i + 1 < argc)
|
|
id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
|
|
else if (arg == "--id2" && i + 1 < argc)
|
|
id2 = static_cast<uint8_t>(std::stoi(argv[++i]));
|
|
else if (arg == "--track_width" && i + 1 < argc)
|
|
track_width = std::stof(argv[++i]);
|
|
else if (arg == "--wheelbase" && i + 1 < argc)
|
|
wheelbase = std::stof(argv[++i]);
|
|
else if (arg == "--radius" && i + 1 < argc)
|
|
wheel_radius = std::stof(argv[++i]);
|
|
else if (arg == "--bumper" && i + 1 < argc)
|
|
bumper_offset = std::stof(argv[++i]);
|
|
else if (arg == "--accel" && i + 1 < argc)
|
|
accel_rate = std::stof(argv[++i]);
|
|
else if (arg == "--decel" && i + 1 < argc)
|
|
decel_rate = std::stof(argv[++i]);
|
|
else if (arg == "--jerk" && i + 1 < argc)
|
|
jerk_rate = std::stof(argv[++i]);
|
|
else if (arg == "--spin_jerk" && i + 1 < argc)
|
|
spin_jerk_rate = std::stof(argv[++i]);
|
|
else if (arg == "--spin_arc_bias" && i + 1 < argc)
|
|
spin_arc_bias = std::stof(argv[++i]);
|
|
else if (arg == "--min_active_rpm" && i + 1 < argc)
|
|
min_active_rpm = std::stof(argv[++i]);
|
|
else if (arg == "--airborne_current_ratio" && i + 1 < argc)
|
|
airborne_current_ratio = std::stof(argv[++i]);
|
|
else if (arg == "--stall_vel_ratio" && i + 1 < argc)
|
|
stall_vel_ratio = std::stof(argv[++i]);
|
|
else if (arg == "--stall_current_a" && i + 1 < argc)
|
|
stall_current_a = std::stof(argv[++i]);
|
|
else if (arg == "--airborne_debounce_ticks" && i + 1 < argc)
|
|
airborne_debounce_ticks = std::stoi(argv[++i]);
|
|
else if (arg == "--log" && i + 1 < argc)
|
|
log_path = argv[++i];
|
|
else if (arg == "--max_current_a" && i + 1 < argc)
|
|
max_current_a = std::stof(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,"
|
|
"err_fl,err_fr,err_rl,err_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;
|
|
}
|
|
|
|
// 현재 드라이버에 실제로 설정된 정격/최대전류를 읽어와서 보여준다
|
|
// (문서 기본값이 아니라 실제 하드웨어에 저장된 값을 확인하기 위함).
|
|
// --max_current_a가 지정되면 4바퀴 최대전류를 그 값으로 낮춘다.
|
|
if (!bcast_mode) {
|
|
uint16_t f_rated_l, f_max_l, f_rated_r, f_max_r;
|
|
if (driver_front.readCurrentLimits(f_rated_l, f_max_l, f_rated_r,
|
|
f_max_r)) {
|
|
std::cout << " - 전방 드라이버 전류설정: 정격 L" << f_rated_l * 0.1f
|
|
<< "A/R" << f_rated_r * 0.1f << "A | 최대 L" << f_max_l * 0.1f
|
|
<< "A/R" << f_max_r * 0.1f << "A\n";
|
|
} else {
|
|
std::cout << "[경고] 전방 드라이버 전류설정 조회 실패\n";
|
|
}
|
|
if (driver_rear_ptr) {
|
|
uint16_t r_rated_l, r_max_l, r_rated_r, r_max_r;
|
|
if (driver_rear_ptr->readCurrentLimits(r_rated_l, r_max_l, r_rated_r,
|
|
r_max_r)) {
|
|
std::cout << " - 후방 드라이버 전류설정: 정격 L" << r_rated_l * 0.1f
|
|
<< "A/R" << r_rated_r * 0.1f << "A | 최대 L"
|
|
<< r_max_l * 0.1f << "A/R" << r_max_r * 0.1f << "A\n";
|
|
} else {
|
|
std::cout << "[경고] 후방 드라이버 전류설정 조회 실패\n";
|
|
}
|
|
}
|
|
|
|
if (max_current_a >= 0.0f) {
|
|
uint16_t new_max_01a = static_cast<uint16_t>(max_current_a * 10.0f);
|
|
driver_front.setMaxCurrent(new_max_01a);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->setMaxCurrent(new_max_01a);
|
|
std::cout << " - 최대전류를 " << max_current_a << "A로 낮춤(4바퀴 동일)\n";
|
|
}
|
|
}
|
|
|
|
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;
|
|
// 착지(airborne 해제) 직후 한 틱 동안: 차체는 이미 그 속도로 움직이고
|
|
// 있는데 cmd는 정지출발용 accel_rate로 천천히 재가속되면, 그 사이 바퀴가
|
|
// 지면 이동속도를 못 따라가 끌리며 긁히는 문제가 있어 즉시 target으로
|
|
// 스냅시켜 지연을 없앤다.
|
|
bool just_landed_fl = false, just_landed_fr = false;
|
|
bool just_landed_rl = false, just_landed_rr = false;
|
|
char wheel_status_str[5] = "OOOO";
|
|
bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부
|
|
|
|
while (g_running) {
|
|
SDL_Event event;
|
|
while (SDL_PollEvent(&event)) {
|
|
if (event.type == SDL_JOYBUTTONDOWN) {
|
|
if (event.jbutton.button == 0) { // A 버튼: 트립 리셋
|
|
trip_initialized = false;
|
|
} else if (event.jbutton.button ==
|
|
2) { // X 버튼: 라이다 장애물 회피 토글
|
|
use_lidar = !use_lidar;
|
|
std::cout << "\n[정보] 라이다 회피 기능: "
|
|
<< (use_lidar ? "ON" : "OFF") << std::endl;
|
|
} else if (event.jbutton.button == 1 ||
|
|
event.jbutton.button == 6) { // B or Back
|
|
g_running = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
Sint16 lx_raw = SDL_JoystickGetAxis(joystick, 0);
|
|
Sint16 ly_raw = SDL_JoystickGetAxis(joystick, 1);
|
|
Sint16 rx_raw = SDL_JoystickGetAxis(joystick, 3);
|
|
|
|
float ly = applyDeadzone(-static_cast<float>(ly_raw) / 32767.0f);
|
|
float lx = applyDeadzone(static_cast<float>(lx_raw) / 32767.0f);
|
|
float rx = applyDeadzone(static_cast<float>(rx_raw) / 32767.0f);
|
|
|
|
if (std::abs(lx) < 0.15f)
|
|
lx = 0.0f;
|
|
if (std::abs(rx) < 0.15f)
|
|
rx = 0.0f;
|
|
|
|
float v_x = ly * max_v; // 선속도 m/s
|
|
float omega = 0.0f; // 각속도 rad/s
|
|
|
|
// ---------------------------------------------------------------------
|
|
// LiDAR Obstacle Avoidance & Stopping Logic Integration
|
|
// (요청에 따라 후진 시 측면 회피는 미적용 - 전진 시에만 좌우 회피 동작)
|
|
// ---------------------------------------------------------------------
|
|
std::string lidar_telemetry = "LIDAR:OFF";
|
|
if (use_lidar) {
|
|
ObstacleStatus obs = lidar_detector.getStatus();
|
|
if (obs.connected) {
|
|
float speed_scale = 1.0f;
|
|
float avoid_omega_offset = 0.0f;
|
|
|
|
// 1. Forward / Backward Automatic Stopping
|
|
if (v_x > 0.01f) {
|
|
if (obs.min_dist_front < front_stop_dist) {
|
|
speed_scale = 0.0f; // Complete forward stop
|
|
} else if (obs.min_dist_front < front_warn_dist) {
|
|
speed_scale = (obs.min_dist_front - front_stop_dist) /
|
|
(front_warn_dist - front_stop_dist);
|
|
speed_scale = std::clamp(speed_scale, 0.0f, 1.0f);
|
|
}
|
|
} else if (v_x < -0.01f) {
|
|
if (obs.min_dist_rear < front_stop_dist) {
|
|
speed_scale = 0.0f; // Complete backward stop
|
|
}
|
|
}
|
|
|
|
// 2. Side Dodge Steering (전진 중에만 동작)
|
|
if (v_x > 0.01f && rx == 0.0f) {
|
|
float right_intensity = 0.0f;
|
|
float left_intensity = 0.0f;
|
|
|
|
if (obs.min_dist_right < side_dodge_dist) {
|
|
right_intensity = (side_dodge_dist - obs.min_dist_right) /
|
|
(side_dodge_dist - side_stop_dist);
|
|
right_intensity = std::clamp(right_intensity, 0.0f, 1.0f);
|
|
}
|
|
if (obs.min_dist_left < side_dodge_dist) {
|
|
left_intensity = (side_dodge_dist - obs.min_dist_left) /
|
|
(side_dodge_dist - side_stop_dist);
|
|
left_intensity = std::clamp(left_intensity, 0.0f, 1.0f);
|
|
}
|
|
|
|
// Right obstacle -> Steer Left (+omega)
|
|
// Left obstacle -> Steer Right (-omega)
|
|
avoid_omega_offset =
|
|
(right_intensity - left_intensity) * max_dodge_omega;
|
|
}
|
|
|
|
v_x *= speed_scale;
|
|
|
|
char lbuf[128];
|
|
snprintf(lbuf, sizeof(lbuf), "F:%4.2fm|L:%4.2fm|R:%4.2fm %s",
|
|
obs.min_dist_front > 99.0f ? 9.99f : obs.min_dist_front,
|
|
obs.min_dist_left > 99.0f ? 9.99f : obs.min_dist_left,
|
|
obs.min_dist_right > 99.0f ? 9.99f : obs.min_dist_right,
|
|
avoid_omega_offset > 0.05f
|
|
? "[Dodge L]"
|
|
: (avoid_omega_offset < -0.05f
|
|
? "[Dodge R]"
|
|
: (speed_scale < 0.01f ? "[STOP!]" : "[OK]")));
|
|
lidar_telemetry = lbuf;
|
|
|
|
// Add avoidance offset to user steering
|
|
omega += avoid_omega_offset;
|
|
} else {
|
|
lidar_telemetry = "LIDAR:WAITING";
|
|
}
|
|
}
|
|
|
|
// 제자리 회전 여부: 왼쪽 스틱 전후(ly) 입력이 완전히 중립일 때만 진입한다.
|
|
// 그렇지 않으면(주행 중 오른쪽 스틱이 드리프트 등으로 미세하게라도 감지될
|
|
// 경우) 전진 입력이 통째로 무시되고 제자리 회전 로직으로 튀어버려, 커브
|
|
// 선회 중에 갑자기 이상하게 회전하려는 문제가 생긴다. ly는 applyDeadzone을
|
|
// 거쳐 데드존 이하일 때 정확히 0.0f를 반환하므로 등호 비교가 안전하다.
|
|
bool is_spin_turn = std::abs(rx) > 0.0f && ly == 0.0f;
|
|
|
|
if (is_spin_turn) {
|
|
// 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합
|
|
omega = -rx * (max_spin_v / (effective_w / 2.0f));
|
|
float v_bias = std::abs(rx) * max_spin_v * spin_arc_bias;
|
|
float v_l = v_bias - omega * (effective_w / 2.0f);
|
|
float v_r = v_bias + omega * (effective_w / 2.0f);
|
|
|
|
target_fl = v_l * rpm_per_ms;
|
|
target_fr = -v_r * rpm_per_ms;
|
|
target_rl = v_l * rpm_per_ms;
|
|
target_rr = -v_r * rpm_per_ms;
|
|
} else {
|
|
// 직진 및 차동 선회 Kinematics
|
|
if (std::abs(rx) == 0.0f && std::abs(lx) == 0.0f &&
|
|
std::abs(omega) > 0.0f) {
|
|
// Automated avoidance steering active
|
|
float v_l = v_x - omega * k_skid;
|
|
float v_r = v_x + omega * k_skid;
|
|
target_fl = v_l * rpm_per_ms;
|
|
target_fr = -v_r * rpm_per_ms;
|
|
target_rl = v_l * rpm_per_ms;
|
|
target_rr = -v_r * rpm_per_ms;
|
|
} else {
|
|
// 선회 강도(omega)는 반드시 "현재 전진 속도(v_x)"에 비례해야 커브
|
|
// 반경이 스로틀과 무관하게 일정하게 유지된다. 이전 구현은 상수인
|
|
// max_v(최고속도)를 기준으로 omega를 계산해서, 저속 주행 중에는
|
|
// v_x가 작은데도 omega*k_skid 항은 고속 기준 그대로 커서 안쪽
|
|
// 바퀴(v_l 또는 v_r) 속도 부호가 뒤집혀 후진해버리는 문제가 있었다.
|
|
// (예: 저속으로 스틱을 왼쪽으로 조금만 꺾어도 로봇이 제자리 회전
|
|
// 하듯 튀거나 뒤로 갔다 앞으로 갔다 하는 증상)
|
|
omega += -lx * (std::abs(v_x) / (effective_w / 2.0f));
|
|
float v_l = v_x - omega * k_skid;
|
|
float v_r = v_x + omega * k_skid;
|
|
|
|
target_fl = v_l * rpm_per_ms;
|
|
target_fr = -v_r * rpm_per_ms;
|
|
target_rl = v_l * rpm_per_ms;
|
|
target_rr = -v_r * rpm_per_ms;
|
|
}
|
|
}
|
|
|
|
// 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 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") {
|
|
// 착지 직후: 정지출발용 저크 램프를 건너뛰고 차체가 이미 내고 있는
|
|
// 속도로 즉시 맞춰 지면과의 미끄러짐(끌림)을 최소화한다.
|
|
if (just_landed_fl) {
|
|
cmd_fl = target_fl;
|
|
accel_fl = 0.0f;
|
|
just_landed_fl = false;
|
|
}
|
|
if (just_landed_fr) {
|
|
cmd_fr = target_fr;
|
|
accel_fr = 0.0f;
|
|
just_landed_fr = false;
|
|
}
|
|
if (just_landed_rl) {
|
|
cmd_rl = target_rl;
|
|
accel_rl = 0.0f;
|
|
just_landed_rl = false;
|
|
}
|
|
if (just_landed_rr) {
|
|
cmd_rr = target_rr;
|
|
accel_rr = 0.0f;
|
|
just_landed_rr = false;
|
|
}
|
|
|
|
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;
|
|
uint16_t err_f_l = 0, err_f_r = 0, err_r_l = 0, err_r_r = 0;
|
|
|
|
driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick, fl_amp, fr_amp,
|
|
err_f_l, err_f_r);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick, rl_amp,
|
|
rr_amp, err_r_l, err_r_r);
|
|
|
|
// 드라이버 알람(과전류/과부하 등) 감지: 새로 발생한 알람만 콘솔에
|
|
// 한 번 크게 출력한다 (매 틱 갱신되는 HUD 줄과 별개의 고정 줄).
|
|
// 알람이 뜨면 드라이버가 명령을 무시하는 잠금 상태가 되어 소프트웨어
|
|
// 재시작만으로는 안 풀리는 경우가 많다(하드웨어 클리어/재활성화 필요).
|
|
bool fault_now = err_f_l || err_f_r || err_r_l || err_r_r;
|
|
if (fault_now && !fault_active) {
|
|
std::cout << "\n\n[!!! 드라이버 알람 발생 !!!] "
|
|
<< "전방L:" << decodeDriverError(err_f_l)
|
|
<< " 전방R:" << decodeDriverError(err_f_r)
|
|
<< " 후방L:" << decodeDriverError(err_r_l)
|
|
<< " 후방R:" << decodeDriverError(err_r_r)
|
|
<< " -> 모터 정지, 전원 재시작 또는 알람클리어 필요\n\n";
|
|
}
|
|
fault_active = fault_now;
|
|
|
|
auto comm_end = std::chrono::high_resolution_clock::now();
|
|
float comm_ms =
|
|
std::chrono::duration<float, std::milli>(comm_end - comm_start).count();
|
|
|
|
// 바퀴별 이상(들뜸/걸림) 판정: 속도 폐루프 특성상 "지령 대비 실제속도"
|
|
// 만으로는 무부하(들뜸)를 구분할 수 없다 (드라이버가 부하와 무관하게
|
|
// 지령 RPM을 그대로 추종하려 하므로). 그래서 두 축으로 판정한다.
|
|
// - 걸림/과부하: 실제속도가 지령을 크게 못 따라가면서 전류가 높음 (독립 판정)
|
|
// - 무부하/들뜸: "같은 지령속도를 받는 짝(앞뒤 같은 쪽)" 대비 전류가
|
|
// 비정상적으로 낮음. 좌/우를 통째로 비교하면 정상적인 커브 선회에서
|
|
// 지령속도가 원래 다른 좌우 바퀴를 오탐하므로, 반드시 target_fl=target_rl,
|
|
// target_fr=target_rr 로 지령이 항상 같은 같은쪽 앞뒤 쌍끼리만 비교한다.
|
|
auto classifyStall = [&](float cmd_rpm, float actual_rpm,
|
|
float amp) -> bool {
|
|
float cmd_abs = std::abs(cmd_rpm);
|
|
if (cmd_abs < min_active_rpm)
|
|
return false;
|
|
float vel_ratio = std::abs(actual_rpm) / cmd_abs;
|
|
return vel_ratio < stall_vel_ratio && std::abs(amp) >= stall_current_a;
|
|
};
|
|
bool stall_fl = classifyStall(cmd_fl, fl_fb, fl_amp);
|
|
bool stall_fr = classifyStall(cmd_fr, fr_fb, fr_amp);
|
|
bool stall_rl = classifyStall(cmd_rl, rl_fb, rl_amp);
|
|
bool stall_rr = classifyStall(cmd_rr, rr_fb, rr_amp);
|
|
|
|
bool air_now_fl = false, air_now_rl = false;
|
|
bool air_now_fr = false, air_now_rr = false;
|
|
auto classifyAirPair = [&](float cmd_rpm, float amp_a, float amp_b,
|
|
bool &air_a, bool &air_b) {
|
|
if (std::abs(cmd_rpm) < min_active_rpm)
|
|
return; // 정지/저속 중이면 판정 보류
|
|
float a = std::abs(amp_a), b = std::abs(amp_b);
|
|
float hi = std::max(a, b);
|
|
if (hi < 0.5f)
|
|
return; // 둘 다 거의 무전류(관성 주행 등)면 판정 보류
|
|
if (a < hi * airborne_current_ratio)
|
|
air_a = true;
|
|
if (b < hi * airborne_current_ratio)
|
|
air_b = true;
|
|
};
|
|
classifyAirPair(cmd_fl, fl_amp, rl_amp, air_now_fl, air_now_rl);
|
|
classifyAirPair(cmd_fr, fr_amp, rr_amp, air_now_fr, air_now_rr);
|
|
|
|
// 디바운스: 노이즈성 순간 전류 편차로 목표속도가 즉시 0으로 깎이지 않도록
|
|
// 연속 airborne_debounce_ticks 틱 이상 지속될 때만 실제로 개입한다.
|
|
air_count_fl = air_now_fl ? air_count_fl + 1 : 0;
|
|
air_count_fr = air_now_fr ? air_count_fr + 1 : 0;
|
|
air_count_rl = air_now_rl ? air_count_rl + 1 : 0;
|
|
air_count_rr = air_now_rr ? air_count_rr + 1 : 0;
|
|
|
|
bool prev_airborne_fl = airborne_fl;
|
|
bool prev_airborne_fr = airborne_fr;
|
|
bool prev_airborne_rl = airborne_rl;
|
|
bool prev_airborne_rr = airborne_rr;
|
|
|
|
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;
|
|
|
|
// 방금 착지(뜬 상태 -> 정상)한 바퀴는 다음 틱에 cmd를 target으로 즉시
|
|
// 스냅해 재가속 지연을 없앤다.
|
|
if (prev_airborne_fl && !airborne_fl)
|
|
just_landed_fl = true;
|
|
if (prev_airborne_fr && !airborne_fr)
|
|
just_landed_fr = true;
|
|
if (prev_airborne_rl && !airborne_rl)
|
|
just_landed_rl = true;
|
|
if (prev_airborne_rr && !airborne_rr)
|
|
just_landed_rr = true;
|
|
|
|
auto statusChar = [](bool air, bool stall) {
|
|
return air ? 'A' : (stall ? 'S' : 'O');
|
|
};
|
|
wheel_status_str[0] = statusChar(airborne_fl, stall_fl);
|
|
wheel_status_str[1] = statusChar(airborne_fr, stall_fr);
|
|
wheel_status_str[2] = statusChar(airborne_rl, stall_rl);
|
|
wheel_status_str[3] = statusChar(airborne_rr, stall_rr);
|
|
|
|
float meters_per_tick = (2.0f * PI_VAL * wheel_radius) / 16384.0f;
|
|
if (!trip_initialized && (fl_tick != 0 || fr_tick != 0)) {
|
|
start_fl = fl_tick;
|
|
start_fr = fr_tick;
|
|
start_rl = rl_tick;
|
|
start_rr = rr_tick;
|
|
trip_initialized = true;
|
|
}
|
|
|
|
int32_t delta_fl = std::abs(fl_tick - start_fl);
|
|
int32_t delta_fr = std::abs(fr_tick - start_fr);
|
|
int32_t delta_rl = std::abs(rl_tick - start_rl);
|
|
int32_t delta_rr = std::abs(rr_tick - start_rr);
|
|
|
|
float dist_fl = delta_fl * meters_per_tick;
|
|
float dist_fr = delta_fr * meters_per_tick;
|
|
float dist_rl = delta_rl * meters_per_tick;
|
|
float dist_rr = delta_rr * meters_per_tick;
|
|
|
|
// 중앙값(4개 중 가운데 2개 평균) 기반 주행거리 산출: 웅덩이 등으로 바퀴
|
|
// 하나가 지면에서 떨어져 엔코더만 헛돌 때, 그 이상치 값이 평균(mean)을
|
|
// 오염시키지 않도록 최댓값/최솟값을 자동으로 배제함.
|
|
float dist_sorted[4] = {dist_fl, dist_fr, dist_rl, dist_rr};
|
|
std::sort(dist_sorted, dist_sorted + 4);
|
|
float dist_axle = (dist_sorted[1] + dist_sorted[2]) / 2.0f;
|
|
float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);
|
|
(void)dist_bumper;
|
|
|
|
if (log_stream.is_open()) {
|
|
float t_ms = std::chrono::duration<float, std::milli>(
|
|
std::chrono::steady_clock::now() - log_start_time)
|
|
.count();
|
|
log_stream << t_ms << ',' << state << ',' << ly << ',' << lx << ','
|
|
<< rx << ',' << cmd_fl << ',' << cmd_fr << ',' << cmd_rl
|
|
<< ',' << cmd_rr << ',' << fl_fb << ',' << fr_fb << ','
|
|
<< rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp
|
|
<< ',' << rl_amp << ',' << rr_amp << ',' << wheel_status_str[0]
|
|
<< ',' << wheel_status_str[1] << ',' << wheel_status_str[2]
|
|
<< ',' << wheel_status_str[3] << ',' << err_f_l << ','
|
|
<< err_f_r << ',' << err_r_l << ',' << err_r_r << ','
|
|
<< 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%s | %4.1fms",
|
|
state.c_str(), dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp,
|
|
rl_amp, rr_amp, wheel_status_str,
|
|
fault_active ? " [!!알람!!]" : "", comm_ms);
|
|
fflush(stdout);
|
|
|
|
std::this_thread::sleep_for(
|
|
std::chrono::microseconds(static_cast<int>(dt * 1000000)));
|
|
}
|
|
|
|
std::cout << "\n\n[정보] C++ 4WD 안전 정지 및 브레이크 잠금 처리 중...\n";
|
|
driver_front.setRPMs(0, 0);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->setRPMs(0, 0);
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
|
driver_front.setBrakes(true);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->setBrakes(true);
|
|
|
|
if (use_lidar)
|
|
lidar_detector.stop();
|
|
|
|
if (log_stream.is_open()) {
|
|
log_stream.close();
|
|
std::cout << "[정보] CSV 로그 저장 완료: " << log_path << "\n";
|
|
}
|
|
|
|
if (joystick)
|
|
SDL_JoystickClose(joystick);
|
|
SDL_Quit();
|
|
|
|
std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
|
|
return 0;
|
|
} |