robin
d5a47881ac
Steering (CH1): omega was computed with the opposite sign from the convention used everywhere else in this file (+omega=left, -omega=right, per the LiDAR avoidance comments and the prior Xbox joystick code), so pushing the stick left steered the robot right and vice versa. Flipped the sign at the single point omega is derived from the stick so curve-turn and spin-turn both inherit the fix. Throttle/steer anti-corruption filter: once a reading was rejected for exceeding max_slew_per_tick, last_raw_throttle/last_raw_steer never updated, so every subsequent real reading kept failing the same slew check against that now-permanently-stale reference — a livelock. Symptom: CH2 showing neutral (1500) on the HUD while the robot kept driving forward regardless of stick input. Added a max_reject_streak escape hatch: after a few consecutive rejections in a row, treat it as a real fast stick movement rather than a one-off corrupted frame and resync to the latest value. Verified working on hardware. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
1843 lines
75 KiB
C++
1843 lines
75 KiB
C++
#include <algorithm>
|
|
#include <array>
|
|
#include <atomic>
|
|
#include <cerrno>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <csignal>
|
|
#include <fcntl.h>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <map>
|
|
#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;
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// RadioMaster Pocket + XR1 V1.0 RC 수신기 (USB-to-TTL 시리얼) 입력 모듈
|
|
// 실기 캡처로 확인한 실제 프로토콜: CRSF(Crossfire), 420000bps, 8N1.
|
|
// 프레임: [0]=Sync(0xC8) [1]=Length [2]=FrameType [3..]=Payload [-1]=CRC8
|
|
// FrameType 0x16(RC_CHANNELS_PACKED)의 22바이트 payload에 16채널이 11비트씩
|
|
// 리틀엔디안으로 패킹되어 있다 (raw 172~1811 -> 1000~2000us로 선형 변환).
|
|
// 참고: mac/mac_code (맥에서 정상 동작 확인된 레퍼런스 파이썬 스크립트)와
|
|
// 동일한 파싱 방식을 따른다 — 실측 결과 CRC8이 표준 DVB-S2(poly 0xD5) 값과
|
|
// 맞지 않아(송신기별 구현 차이로 추정) 레퍼런스와 동일하게 CRC는 검증하지
|
|
// 않고 Sync+Length 기반 프레이밍만 신뢰한다.
|
|
// -----------------------------------------------------------------------------
|
|
#ifndef BOTHER
|
|
#define BOTHER 0010000 // 커널 termbits.h: CBAUDEX와 동일값, 임의 보드레이트 지정용
|
|
#endif
|
|
struct termios2 { // <asm/termbits.h>의 커널 ABI와 동일 레이아웃 (glibc의
|
|
// struct termios와 이름 충돌을 피하기 위해 직접 선언).
|
|
// 420000bps처럼 표준 B-상수가 없는 보드레이트를
|
|
// TCSETS2/BOTHER ioctl로 설정하기 위해 필요하다.
|
|
tcflag_t c_iflag, c_oflag, c_cflag, c_lflag;
|
|
cc_t c_line;
|
|
cc_t c_cc[19];
|
|
speed_t c_ispeed, c_ospeed;
|
|
};
|
|
|
|
struct RcConfig {
|
|
std::string port = "/dev/ttyUSB0"; // 현재 고정 배선: RC 수신기(XR1) = ttyUSB0
|
|
int baud = 420000; // CRSF 표준 보드레이트
|
|
|
|
int ch_steer = 1; // CH1: 좌우 조향
|
|
int ch_throttle = 2; // CH2: 전후진
|
|
int ch_brake = 5; // CH5: 비상 브레이크
|
|
int ch_speed = 6; // CH6: 속도 모드
|
|
|
|
// 실기 캡처로 확인한 실측값 (좌 1005 ~ 중 1481~1503 ~ 우 2000)
|
|
int steer_min = 1005, steer_center_lo = 1481, steer_center_hi = 1503,
|
|
steer_max = 2000;
|
|
// 실측값: 후진끝 1005 ~ 중 1500 ~ 전진끝 2000 (CH2는 값이 높을수록 전진 —
|
|
// CH1과 반대 방향이라 아래 3.3 매핑 로직도 그에 맞춰 부호가 반대다)
|
|
int throttle_min = 1005, throttle_center_lo = 1480, throttle_center_hi = 1520,
|
|
throttle_max = 2000;
|
|
|
|
int brake_threshold = 1500; // 이상이면 브레이크 ON
|
|
int speed_threshold = 1500; // 이상이면 저속 모드
|
|
|
|
// CH1/CH2(연속 아날로그 스틱) 슬루레이트 한계: 메인 루프 한 틱(~10ms)
|
|
// 사이에 raw 값이 이 이상 튀면 사람이 스틱으로 낼 수 없는 변화로 보고
|
|
// 손상된 프레임으로 간주해 버린다. CH5/CH6(스위치)는 원래 한 번에 크게
|
|
// 뛰는 게 정상이라 이 제한을 적용하지 않는다.
|
|
int max_slew_per_tick = 200;
|
|
// 슬루레이트 거부가 연속으로 이 틱 수 이상 이어지면(=한 번의 손상
|
|
// 프레임이 아니라 실제로 스틱이 그만큼 빠르게/멀리 움직인 상황으로
|
|
// 판단) 그냥 최신값을 그대로 받아들여 재동기화한다. 이 escape hatch가
|
|
// 없으면 range는 정상인데 slew만 거부된 값 하나가 last_raw_*를 그
|
|
// 자리에 영원히 고정시켜버려서, 이후 들어오는 진짜 값들도 그 고정된
|
|
// 기준과 계속 200 이상 차이나 버려 채널이 그 값에 영구히 멈춰버린다.
|
|
int max_reject_streak = 3;
|
|
|
|
float v_max_low = 0.3f; // m/s (CH6 저속 모드)
|
|
float v_max_high = 1.5f; // m/s (CH6 고속 모드)
|
|
|
|
int failsafe_timeout_ms = 200; // 이 시간 이상 신호 미수신 시 비상정지
|
|
};
|
|
|
|
class RcReceiver {
|
|
public:
|
|
explicit RcReceiver(const RcConfig &cfg) : cfg_(cfg) {}
|
|
~RcReceiver() { stop(); }
|
|
|
|
void start() {
|
|
running_ = true;
|
|
thread_ = std::thread(&RcReceiver::run, this);
|
|
}
|
|
|
|
void stop() {
|
|
running_ = false;
|
|
if (thread_.joinable())
|
|
thread_.join();
|
|
closePort();
|
|
}
|
|
|
|
// 채널 raw 값 조회. 아직 한 번도 수신하지 못했으면 false.
|
|
bool getChannel(int ch, int &out_val) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
auto it = channels_.find(ch);
|
|
if (it == channels_.end())
|
|
return false;
|
|
out_val = it->second;
|
|
return true;
|
|
}
|
|
|
|
// 포트가 열려 있고, failsafe_timeout_ms 이내에 유효한 패킷을 수신했는가.
|
|
bool isConnected() {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
if (!port_open_ || channels_.empty())
|
|
return false;
|
|
auto now = std::chrono::steady_clock::now();
|
|
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
now - last_rx_time_)
|
|
.count() <= cfg_.failsafe_timeout_ms;
|
|
}
|
|
|
|
private:
|
|
RcConfig cfg_;
|
|
std::atomic<bool> running_{false};
|
|
std::thread thread_;
|
|
int fd_ = -1;
|
|
|
|
std::mutex mutex_;
|
|
std::map<int, int> channels_;
|
|
std::chrono::steady_clock::time_point last_rx_time_{};
|
|
bool port_open_ = false;
|
|
|
|
std::vector<uint8_t> frame_buf_;
|
|
|
|
// 채널별 미디언(median-of-3) 필터 히스토리. 이 송신기는 CRC/체크섬이
|
|
// 표준 방식으로 검증되지 않아(전수조사로 확인) 단발성 손상 프레임이
|
|
// 섞여 들어올 수 있는데, 최근 3개 raw 값 중 가운데 값을 채택하면 3개
|
|
// 중 하나가 손상돼도 자동으로 걸러진다. 스위치 채널도 전환 시 최대
|
|
// 1프레임(~4ms)만 지연될 뿐이라 안전하게 전 채널에 적용 가능하다.
|
|
std::map<int, std::array<int, 2>> raw_history_;
|
|
std::map<int, int> history_count_;
|
|
|
|
static int median3(int a, int b, int c) {
|
|
if ((a <= b && b <= c) || (c <= b && b <= a))
|
|
return b;
|
|
if ((b <= a && a <= c) || (c <= a && a <= b))
|
|
return a;
|
|
return c;
|
|
}
|
|
|
|
static constexpr uint8_t CRSF_SYNC = 0xC8;
|
|
static constexpr uint8_t CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16;
|
|
static constexpr size_t CRSF_MAX_FRAME_LEN = 64; // 스펙상 프레임은 훨씬 작음
|
|
|
|
bool openPort() {
|
|
fd_ = open(cfg_.port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
|
if (fd_ < 0)
|
|
return false;
|
|
|
|
fcntl(fd_, F_SETFL, O_NONBLOCK);
|
|
|
|
// CRSF 표준 420000bps는 termios의 표준 B-상수 목록에 없어 TCSETS2 +
|
|
// BOTHER + c_ispeed/c_ospeed(임의 정수 보드레이트) ioctl로 설정한다.
|
|
struct termios2 tio;
|
|
if (ioctl(fd_, TCGETS2, &tio) < 0) {
|
|
close(fd_);
|
|
fd_ = -1;
|
|
return false;
|
|
}
|
|
|
|
tio.c_cflag &= ~CBAUD;
|
|
tio.c_cflag |= BOTHER;
|
|
tio.c_ispeed = static_cast<speed_t>(cfg_.baud);
|
|
tio.c_ospeed = static_cast<speed_t>(cfg_.baud);
|
|
|
|
tio.c_cflag &= ~PARENB;
|
|
tio.c_cflag &= ~CSTOPB;
|
|
tio.c_cflag &= ~CSIZE;
|
|
tio.c_cflag |= CS8;
|
|
tio.c_cflag &= ~CRTSCTS;
|
|
tio.c_cflag |= CREAD | CLOCAL;
|
|
|
|
// 바이너리 프로토콜이므로 raw 모드(비정규/논캐노니컬)로 읽는다.
|
|
tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | IEXTEN);
|
|
tio.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
|
|
ISTRIP | INLCR | IGNCR | ICRNL);
|
|
tio.c_oflag &= ~OPOST;
|
|
tio.c_cc[VMIN] = 0;
|
|
tio.c_cc[VTIME] = 0;
|
|
|
|
if (ioctl(fd_, TCSETS2, &tio) < 0) {
|
|
close(fd_);
|
|
fd_ = -1;
|
|
return false;
|
|
}
|
|
tcflush(fd_, TCIOFLUSH);
|
|
return true;
|
|
}
|
|
|
|
void closePort() {
|
|
if (fd_ >= 0) {
|
|
close(fd_);
|
|
fd_ = -1;
|
|
}
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
port_open_ = false;
|
|
}
|
|
|
|
// 축적된 바이트 버퍼에서 CRSF 프레임을 최대한 뽑아낸다. Sync 바이트를
|
|
// 찾고, 선언된 Length만큼 버퍼에 데이터가 쌓일 때까지 기다린 뒤
|
|
// RC_CHANNELS_PACKED(0x16) 프레임이면 16채널(11비트씩)을 언패킹한다.
|
|
// (CRC8은 이 송신기에서 표준 DVB-S2 값과 맞지 않아 검증하지 않음 — 실기
|
|
// 캡처로 확인된 mac/mac_code 레퍼런스와 동일한 방식.)
|
|
void parseCrsfBuffer() {
|
|
std::map<int, int> parsed;
|
|
size_t i = 0;
|
|
while (i < frame_buf_.size()) {
|
|
if (frame_buf_[i] != CRSF_SYNC) {
|
|
++i;
|
|
continue;
|
|
}
|
|
if (i + 1 >= frame_buf_.size())
|
|
break; // length 바이트 대기
|
|
|
|
uint8_t length = frame_buf_[i + 1];
|
|
size_t total_len = static_cast<size_t>(length) + 2;
|
|
if (length < 2 || total_len > CRSF_MAX_FRAME_LEN) {
|
|
// Sync처럼 보이는 노이즈 바이트: 1바이트만 버리고 재동기화
|
|
++i;
|
|
continue;
|
|
}
|
|
if (i + total_len > frame_buf_.size())
|
|
break; // 프레임 전체 수신 대기
|
|
|
|
uint8_t frame_type = frame_buf_[i + 2];
|
|
if (frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED && length >= 23) {
|
|
// payload = frame_buf_[i+3 .. i+total_len-2] (CRC 바이트 제외 22바이트
|
|
// = 176비트 = 16채널×11비트). uint64_t 하나에 통째로 담으면 64비트를
|
|
// 넘는 시프트가 발생해 정의되지 않은 동작(UB)이 되므로, 32비트 롤링
|
|
// 비트 누산기로 바이트를 하나씩 채워가며 11비트씩 뽑아낸다.
|
|
uint32_t bit_accum = 0;
|
|
int bit_count = 0;
|
|
int byte_idx = 0;
|
|
for (int ch = 0; ch < 16; ++ch) {
|
|
while (bit_count < 11) {
|
|
bit_accum |= static_cast<uint32_t>(frame_buf_[i + 3 + byte_idx])
|
|
<< bit_count;
|
|
bit_count += 8;
|
|
++byte_idx;
|
|
}
|
|
int raw_val = static_cast<int>(bit_accum & 0x7FFU);
|
|
bit_accum >>= 11;
|
|
bit_count -= 11;
|
|
int us_val = static_cast<int>(
|
|
std::lround((raw_val - 172) * 1000.0 / 1639.0 + 1000.0));
|
|
|
|
// 미디언-3 필터: 최근 2개 raw 값 + 이번 값 중 가운데 값을 채택.
|
|
// 히스토리는 항상 raw 값으로 갱신한다(필터링된 결과가 아니라).
|
|
int ch_num = ch + 1;
|
|
std::array<int, 2> &hist = raw_history_[ch_num];
|
|
int &count = history_count_[ch_num];
|
|
int filtered = us_val;
|
|
if (count >= 2) {
|
|
filtered = median3(hist[0], hist[1], us_val);
|
|
} else {
|
|
++count; // 워밍업 구간(처음 2개)은 그대로 통과
|
|
}
|
|
hist[0] = hist[1];
|
|
hist[1] = us_val;
|
|
parsed[ch_num] = filtered; // CH1..CH16 (1-based)
|
|
}
|
|
}
|
|
i += total_len;
|
|
}
|
|
|
|
// 처리하지 못한 잔여 바이트(다음 프레임 일부)만 남기고 버퍼 정리
|
|
frame_buf_.erase(frame_buf_.begin(), frame_buf_.begin() + i);
|
|
if (frame_buf_.size() > CRSF_MAX_FRAME_LEN * 4) // 비정상 누적 방지
|
|
frame_buf_.clear();
|
|
|
|
if (!parsed.empty()) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
for (auto &kv : parsed)
|
|
channels_[kv.first] = kv.second;
|
|
last_rx_time_ = std::chrono::steady_clock::now();
|
|
}
|
|
}
|
|
|
|
void run() {
|
|
// now() - 1001ms로 초기화: time_point::min()을 쓰면 첫 비교에서
|
|
// "now - sentinel"이 부호있는 정수 오버플로우(UB)를 일으켜 -O3에서
|
|
// 재연결 시도 자체가 영구히 스킵될 수 있다.
|
|
auto last_open_attempt =
|
|
std::chrono::steady_clock::now() - std::chrono::milliseconds(1001);
|
|
char buf[256];
|
|
|
|
while (running_) {
|
|
if (fd_ < 0) {
|
|
auto now = std::chrono::steady_clock::now();
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
now - last_open_attempt)
|
|
.count() > 1000) {
|
|
last_open_attempt = now;
|
|
if (openPort()) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
port_open_ = true;
|
|
std::cout << "\n[정보] RC 수신기 포트(" << cfg_.port
|
|
<< ") 연결 성공.\n";
|
|
} else {
|
|
std::cerr << "\n[경고] RC 수신기 포트(" << cfg_.port
|
|
<< ") 열기 실패. 1초 후 재시도.\n";
|
|
}
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
continue;
|
|
}
|
|
|
|
ssize_t n = read(fd_, buf, sizeof(buf));
|
|
if (n > 0) {
|
|
frame_buf_.insert(frame_buf_.end(), buf, buf + n);
|
|
parseCrsfBuffer();
|
|
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
|
|
std::cerr << "\n[경고] RC 수신기 포트 통신 오류. 재연결 시도...\n";
|
|
closePort();
|
|
} else {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
int main(int argc, char **argv) {
|
|
std::signal(SIGINT, signalHandler);
|
|
std::signal(SIGTERM, signalHandler);
|
|
|
|
std::string port1 = "/dev/ttyUSB1"; // 현재 고정 배선: 모터 드라이버 = ttyUSB1
|
|
std::string port2 = "";
|
|
uint8_t id1 = 1;
|
|
uint8_t id2 = 2;
|
|
bool bcast_mode = false;
|
|
RcConfig rc_cfg;
|
|
float max_spin_v = 0.30f; // m/s
|
|
float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm
|
|
float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm
|
|
float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름
|
|
// 가속은 역기전력 보호회로가 하드웨어로 구성되어 목표값에 1:1로 즉시
|
|
// 추종하므로 별도의 가속 한계값이 없다. decel_rate/jerk_rate(및
|
|
// spin_jerk_rate)는 감속에만 적용된다.
|
|
float decel_rate = 180.0f; // RPM/s (최대 감속도 한계)
|
|
float jerk_rate = 600.0f; // RPM/s^2 (직진/커브 선회 감속 저크 한계)
|
|
float spin_jerk_rate = 220.0f; // RPM/s^2 (제자리 회전 감속 저크 한계)
|
|
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 == "--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;
|
|
else if (arg == "--rc_port" && i + 1 < argc)
|
|
rc_cfg.port = argv[++i];
|
|
else if (arg == "--rc_baud" && i + 1 < argc)
|
|
rc_cfg.baud = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_ch_steer" && i + 1 < argc)
|
|
rc_cfg.ch_steer = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_ch_throttle" && i + 1 < argc)
|
|
rc_cfg.ch_throttle = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_ch_brake" && i + 1 < argc)
|
|
rc_cfg.ch_brake = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_ch_speed" && i + 1 < argc)
|
|
rc_cfg.ch_speed = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_steer_min" && i + 1 < argc)
|
|
rc_cfg.steer_min = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_steer_center_lo" && i + 1 < argc)
|
|
rc_cfg.steer_center_lo = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_steer_center_hi" && i + 1 < argc)
|
|
rc_cfg.steer_center_hi = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_steer_max" && i + 1 < argc)
|
|
rc_cfg.steer_max = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_throttle_min" && i + 1 < argc)
|
|
rc_cfg.throttle_min = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_throttle_center_lo" && i + 1 < argc)
|
|
rc_cfg.throttle_center_lo = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_throttle_center_hi" && i + 1 < argc)
|
|
rc_cfg.throttle_center_hi = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_throttle_max" && i + 1 < argc)
|
|
rc_cfg.throttle_max = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_brake_threshold" && i + 1 < argc)
|
|
rc_cfg.brake_threshold = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_speed_threshold" && i + 1 < argc)
|
|
rc_cfg.speed_threshold = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_max_slew" && i + 1 < argc)
|
|
rc_cfg.max_slew_per_tick = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_max_reject_streak" && i + 1 < argc)
|
|
rc_cfg.max_reject_streak = std::stoi(argv[++i]);
|
|
else if (arg == "--rc_vmax_low" && i + 1 < argc)
|
|
rc_cfg.v_max_low = std::stof(argv[++i]);
|
|
else if (arg == "--rc_vmax_high" && i + 1 < argc)
|
|
rc_cfg.v_max_high = std::stof(argv[++i]);
|
|
else if (arg == "--rc_failsafe_ms" && i + 1 < argc)
|
|
rc_cfg.failsafe_timeout_ms = std::stoi(argv[++i]);
|
|
else if (arg == "--max_spin_v" && i + 1 < argc)
|
|
max_spin_v = std::stof(argv[++i]);
|
|
}
|
|
|
|
constexpr float PI_VAL = 3.14159265358979323846f;
|
|
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 << " - 최고 속도: 저속 " << rc_cfg.v_max_low << "m/s / 고속 "
|
|
<< rc_cfg.v_max_high << "m/s (CH" << rc_cfg.ch_speed
|
|
<< " 전환) | 가속: 즉시 추종(1:1) | 감속 Jerk: " << jerk_rate
|
|
<< "RPM/s² (제자리회전: " << spin_jerk_rate
|
|
<< "RPM/s²) | 회전 곡선혼합: " << spin_arc_bias * 100.0f << "%\n";
|
|
std::cout << " - RC 수신기: " << rc_cfg.port << " @" << rc_cfg.baud
|
|
<< "bps | 조향 CH" << rc_cfg.ch_steer << " | 스로틀 CH"
|
|
<< rc_cfg.ch_throttle << " | 브레이크 CH" << rc_cfg.ch_brake
|
|
<< " | 속도모드 CH" << rc_cfg.ch_speed
|
|
<< " | Fail-safe: " << rc_cfg.failsafe_timeout_ms << "ms\n";
|
|
std::cout << " - 휠 이상판정: 무부하(들뜸) 전류비 " << airborne_current_ratio
|
|
<< " | 걸림 속도비 " << stall_vel_ratio << " / 전류 "
|
|
<< stall_current_a << "A 이상\n";
|
|
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,rc_steer,rc_throttle,rc_ok,brake,vmax,vx,omega,"
|
|
"ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,"
|
|
"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";
|
|
}
|
|
}
|
|
|
|
// 모터(--port)와 RC 수신기(--rc_port)가 같은 물리 포트를 가리키면 두
|
|
// 스레드가 같은 장치의 read()를 두고 경쟁해 RC 라인 파싱이 깨진다.
|
|
// 흔한 실수(예: 모터 미연결 상태에서 RC 수신기가 ttyUSB0에 꽂혀있는데
|
|
// --rc_port를 기본값 ttyUSB1로 둔 채 실행)를 바로 알아차리도록 경고한다.
|
|
if (port1 == rc_cfg.port) {
|
|
std::cout << "\n[경고] --port(모터, " << port1 << ")와 --rc_port(RC 수신기, "
|
|
<< rc_cfg.port
|
|
<< ")가 동일한 포트로 설정되어 있습니다. 두 기능이 같은 장치를 "
|
|
"동시에 읽으면 RC 데이터 파싱이 깨질 수 있으니 서로 다른 "
|
|
"포트로 지정하세요.\n\n";
|
|
}
|
|
|
|
RcReceiver rc(rc_cfg);
|
|
rc.start();
|
|
std::cout << "[정보] RadioMaster Pocket / XR1 V1.0 RC 수신기 리더 시작 ("
|
|
<< rc_cfg.port << "). 신호 수신 전까지는 안전을 위해 브레이크가 "
|
|
"걸린 상태로 대기합니다.\n";
|
|
|
|
SerialPort sp1, sp2;
|
|
if (!sp1.openPort(port1))
|
|
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 초저지연 루프를 시작합니다. (종료: Ctrl+C, "
|
|
"라이다 회피 " << (use_lidar ? "활성화" : "비활성화")
|
|
<< " 상태로 시작 - 변경하려면 --no_lidar 옵션을 사용하세요)\n\n";
|
|
|
|
float target_fl = 0.0f, target_fr = 0.0f;
|
|
float target_rl = 0.0f, target_rr = 0.0f;
|
|
|
|
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;
|
|
|
|
// 가속: 역기전력 보호회로가 하드웨어로 구성되어 더 이상 소프트웨어로
|
|
// 완만하게 램프업할 필요가 없어져, 조이스틱 목표값에 1:1로 즉시 추종한다.
|
|
// 감속: 기존과 동일하게 저크 제한(jerk-limited) S-curve 유지 — 가속도
|
|
// 자체를 매 틱 jerk_limit 만큼만 변화시켜 서서히 목표 가속도에 도달하게
|
|
// 해서, 급감속 시 타이어 스크럽/전복 위험을 줄인다.
|
|
auto jerkLimitedStep = [dt](float current_vel, float target_vel,
|
|
float ¤t_accel, 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);
|
|
if (is_accelerating) {
|
|
current_accel = 0.0f;
|
|
return target_vel;
|
|
}
|
|
|
|
// 이번 틱에 diff를 없애는데 필요한 가속도(부호 포함)를 물리적 한계로 클램프
|
|
float desired_accel = std::clamp(diff / dt, -max_decel_mag, max_decel_mag);
|
|
|
|
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가 뒤늦게 target을 쫓아가면 그 사이 바퀴가 지면 이동속도를
|
|
// 못 따라가 끌리며 긁히는 문제가 있어 즉시 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; // 드라이버 알람(과전류/과부하 등) 발생 여부
|
|
bool prev_brake_on = false; // 브레이크(CH5/Fail-safe) 직전 틱 상태
|
|
bool prev_rc_ok = false; // RC 연결 직전 틱 상태 (Fail-safe 로그용)
|
|
float omega_max = (effective_w > 0.0f) ? (max_spin_v / (effective_w / 2.0f))
|
|
: 0.0f; // CH1 조향 최대 각속도
|
|
// CRC 미검증으로 인해 손상된 CRSF 프레임이 드물게 통과할 수 있다.
|
|
// (실측 결과 이 송신기는 표준 CRC8 변형 어떤 것과도 안 맞아 — poly
|
|
// 전수조사+여러 바이트 범위로 확인해도 일치율이 최대 33% 수준이라
|
|
// CRC 자체를 신뢰할 수 없다고 판단, 검증 시도를 포기함.) 대신 두 가지
|
|
// 물리적 타당성 검사로 걸러낸다:
|
|
// 1) 캘리브레이션 범위(steer_min~max, throttle_min~max) 밖의 값 거부
|
|
// 2) 슬루레이트: 한 틱(~10ms) 사이에 사람이 스틱으로 낼 수 없을 만큼
|
|
// 크게 튀는 값 거부 (예: 1227 -> 1800 같은 순간 점프). 단, max_reject_streak
|
|
// 틱 연속으로 거부되면(=한 번의 손상 프레임이 아니라 실제 상황) 강제
|
|
// 수락해 재동기화한다 — 그렇지 않으면 last_raw_*가 그 자리에 영구히
|
|
// 고정되어, 이후 들어오는 진짜 값들도 계속 그 고정된 기준과 크게
|
|
// 차이나 버려 채널이 응답 없이 멈춰버린다.
|
|
// 범위 밖 값은 항상 거부, 그 틱은 마지막으로 유효했던 값을 그대로 사용한다.
|
|
int last_raw_steer = rc_cfg.steer_center_lo;
|
|
int last_raw_throttle = 1500;
|
|
bool steer_initialized = false, throttle_initialized = false;
|
|
int steer_reject_streak = 0, throttle_reject_streak = 0;
|
|
|
|
while (g_running) {
|
|
// ---------------------------------------------------------------------
|
|
// RC 수신기 채널 읽기 (doc/joystick.md §2, §3 기능 명세서 기준)
|
|
// ---------------------------------------------------------------------
|
|
int raw_steer = rc_cfg.steer_center_lo, raw_throttle = 1500;
|
|
int raw_brake = 1011, raw_speed_mode = 1011;
|
|
bool rc_ok = rc.isConnected();
|
|
rc.getChannel(rc_cfg.ch_steer, raw_steer);
|
|
rc.getChannel(rc_cfg.ch_throttle, raw_throttle);
|
|
rc.getChannel(rc_cfg.ch_brake, raw_brake);
|
|
rc.getChannel(rc_cfg.ch_speed, raw_speed_mode);
|
|
|
|
bool steer_range_ok =
|
|
raw_steer >= rc_cfg.steer_min && raw_steer <= rc_cfg.steer_max;
|
|
if (!steer_range_ok) {
|
|
raw_steer = last_raw_steer; // 캘리브레이션 범위 밖 -> 항상 거부
|
|
} else {
|
|
bool steer_slew_ok = !steer_initialized ||
|
|
std::abs(raw_steer - last_raw_steer) <=
|
|
rc_cfg.max_slew_per_tick;
|
|
// slew 거부가 max_reject_streak틱 연속되면 진짜 빠른 스틱 조작으로
|
|
// 보고 강제로 재동기화한다 (아래 주석 참고: 그렇지 않으면 last_raw_*가
|
|
// 영구히 고정되어 채널이 멈춰버림).
|
|
if (steer_slew_ok || steer_reject_streak >= rc_cfg.max_reject_streak) {
|
|
last_raw_steer = raw_steer;
|
|
steer_initialized = true;
|
|
steer_reject_streak = 0;
|
|
} else {
|
|
raw_steer = last_raw_steer;
|
|
++steer_reject_streak;
|
|
}
|
|
}
|
|
|
|
bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min &&
|
|
raw_throttle <= rc_cfg.throttle_max;
|
|
if (!throttle_range_ok) {
|
|
raw_throttle = last_raw_throttle;
|
|
} else {
|
|
bool throttle_slew_ok = !throttle_initialized ||
|
|
std::abs(raw_throttle - last_raw_throttle) <=
|
|
rc_cfg.max_slew_per_tick;
|
|
if (throttle_slew_ok || throttle_reject_streak >= rc_cfg.max_reject_streak) {
|
|
last_raw_throttle = raw_throttle;
|
|
throttle_initialized = true;
|
|
throttle_reject_streak = 0;
|
|
} else {
|
|
raw_throttle = last_raw_throttle;
|
|
++throttle_reject_streak;
|
|
}
|
|
}
|
|
|
|
// CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
|
|
// 직접 눈으로 검증할 수 있도록 HUD/CSV에 그대로 노출).
|
|
int raw_ch[9]; // 인덱스 1..8 사용
|
|
for (int ch = 1; ch <= 8; ++ch) {
|
|
if (!rc.getChannel(ch, raw_ch[ch]))
|
|
raw_ch[ch] = 0; // 아직 수신 못한 채널은 0으로 표시
|
|
}
|
|
|
|
if (!rc_ok && prev_rc_ok) {
|
|
std::cout << "\n[경고] RC 신호 Fail-safe 발동 (" << rc_cfg.failsafe_timeout_ms
|
|
<< "ms 이상 미수신) -> 비상 정지.\n";
|
|
}
|
|
prev_rc_ok = rc_ok;
|
|
|
|
// 3.1 브레이크 최우선 로직 (CH5) - 미수신(Fail-safe) 상태도 동일하게 처리
|
|
bool brake_on = !rc_ok || (raw_brake >= rc_cfg.brake_threshold);
|
|
|
|
// 3.2 속도 제한 설정 (CH6)
|
|
float v_max_now = (raw_speed_mode >= rc_cfg.speed_threshold)
|
|
? rc_cfg.v_max_low
|
|
: rc_cfg.v_max_high;
|
|
|
|
float v_x = 0.0f; // 선속도 m/s
|
|
float omega = 0.0f; // 각속도 rad/s
|
|
|
|
if (!brake_on) {
|
|
// 3.3 전후진 속도 계산 (CH2) — 실측 결과 raw 값이 높을수록 전진(+),
|
|
// 낮을수록 후진(-)이라 CH1(조향)과는 반대 부호 방향이다.
|
|
int t = std::clamp(raw_throttle, rc_cfg.throttle_min, rc_cfg.throttle_max);
|
|
if (t >= rc_cfg.throttle_center_lo && t <= rc_cfg.throttle_center_hi) {
|
|
v_x = 0.0f;
|
|
} else if (t < rc_cfg.throttle_center_lo) {
|
|
v_x = -(static_cast<float>(rc_cfg.throttle_center_lo - t) /
|
|
static_cast<float>(rc_cfg.throttle_center_lo - rc_cfg.throttle_min)) *
|
|
v_max_now;
|
|
} else {
|
|
v_x = (static_cast<float>(t - rc_cfg.throttle_center_hi) /
|
|
static_cast<float>(rc_cfg.throttle_max - rc_cfg.throttle_center_hi)) *
|
|
v_max_now;
|
|
}
|
|
|
|
// 3.4 좌우 조향 각속도 계산 (CH1). omega 부호 규약(코드 전역 공통,
|
|
// 자동회피 로직의 "+omega=좌회전/-omega=우회전" 주석과 일치): 스틱을
|
|
// 왼쪽(raw 낮음)으로 밀면 +omega(좌회전), 오른쪽(raw 높음)이면
|
|
// -omega(우회전)여야 한다. 예전엔 부호가 반대로 들어가 있어서 스틱
|
|
// 방향과 실제 회전 방향이 뒤바뀌어 있었다.
|
|
int s = std::clamp(raw_steer, rc_cfg.steer_min, rc_cfg.steer_max);
|
|
if (s >= rc_cfg.steer_center_lo && s <= rc_cfg.steer_center_hi) {
|
|
omega = 0.0f;
|
|
} else if (s < rc_cfg.steer_center_lo) {
|
|
omega = (static_cast<float>(rc_cfg.steer_center_lo - s) /
|
|
static_cast<float>(rc_cfg.steer_center_lo - rc_cfg.steer_min)) *
|
|
omega_max;
|
|
} else {
|
|
omega = -(static_cast<float>(s - rc_cfg.steer_center_hi) /
|
|
static_cast<float>(rc_cfg.steer_max - rc_cfg.steer_center_hi)) *
|
|
omega_max;
|
|
}
|
|
}
|
|
|
|
// ---------------------------------------------------------------------
|
|
// 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 && omega == 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";
|
|
}
|
|
}
|
|
|
|
// 제자리 회전 여부: CH2(전후진) 입력이 완전히 중립(v_x == 0.0f)이면서
|
|
// CH1(조향) 입력이 있을 때만 진입한다. v_x는 데드존 처리 시 정확히
|
|
// 0.0f로 설정되므로 등호 비교가 안전하다.
|
|
bool is_spin_turn = (v_x == 0.0f) && (std::abs(omega) > 0.0f);
|
|
|
|
if (is_spin_turn) {
|
|
// 제자리 회전 (Spin Turn) + 최소 회전반경용 미세 곡선 혼합 (스크럽
|
|
// 마찰 저항 감소를 위해 ICR을 로봇 중심에서 살짝 벗어나게 함)
|
|
float norm = (omega_max > 0.0f)
|
|
? std::clamp(std::abs(omega) / omega_max, 0.0f, 1.0f)
|
|
: 0.0f;
|
|
float v_bias = norm * max_spin_v * spin_arc_bias;
|
|
float v_l = v_bias - omega * (effective_w / 2.0f);
|
|
float v_r = v_bias + omega * (effective_w / 2.0f);
|
|
|
|
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 (CH1 조향 + 라이다 회피 오프셋 포함)
|
|
float v_l = v_x - omega * k_skid;
|
|
float v_r = v_x + omega * k_skid;
|
|
target_fl = v_l * rpm_per_ms;
|
|
target_fr = -v_r * rpm_per_ms;
|
|
target_rl = v_l * rpm_per_ms;
|
|
target_rr = -v_r * rpm_per_ms;
|
|
}
|
|
|
|
// 직전 틱 피드백에서 무부하(들뜸)로 판정된 바퀴는 목표 속도를 0으로 낮춰
|
|
// 헛돌이를 억제한다. 접지력이 회복되어 전류가 정상으로 돌아오면 다음
|
|
// 판정 틱에서 자동으로 해제되어 원래 지령으로 복귀한다.
|
|
if (airborne_fl)
|
|
target_fl = 0.0f;
|
|
if (airborne_fr)
|
|
target_fr = 0.0f;
|
|
if (airborne_rl)
|
|
target_rl = 0.0f;
|
|
if (airborne_rr)
|
|
target_rr = 0.0f;
|
|
|
|
// 3.1 브레이크 최우선 로직 (CH5/Fail-safe): 저크 램프를 건너뛰고 즉시
|
|
// 0으로 스냅 후 브레이크를 잠근다. 해제되면 STOPPED 상태에서 아래
|
|
// 상태머신이 정상적으로 재가속을 시작한다.
|
|
// 전자식 브레이크(솔레노이드)는 오직 CH5로만 제어한다 — 스로틀을 중립으로
|
|
// 되돌려 자연 정지했을 때는 잠그지 않고, CH5를 눌렀을 때만 잠근다.
|
|
if (brake_on && !prev_brake_on) {
|
|
cmd_fl = cmd_fr = cmd_rl = cmd_rr = 0.0f;
|
|
accel_fl = accel_fr = accel_rl = accel_rr = 0.0f;
|
|
driver_front.setRPMs(0, 0);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->setRPMs(0, 0);
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
driver_front.setBrakes(true);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->setBrakes(true);
|
|
state = "STOPPED";
|
|
} else if (!brake_on && prev_brake_on) {
|
|
driver_front.setBrakes(false);
|
|
if (driver_rear_ptr)
|
|
driver_rear_ptr->setBrakes(false);
|
|
}
|
|
prev_brake_on = brake_on;
|
|
|
|
if (brake_on) {
|
|
// target_fl..rr은 이미 0(v_x=omega=0)이며 STOPPED 상태를 유지한다.
|
|
} else if (state == "STOPPED") {
|
|
if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
|
|
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, decel_rate, jerk_now);
|
|
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now);
|
|
cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now);
|
|
cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, 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, jerk_now);
|
|
cmd_fr = jerkLimitedStep(cmd_fr, target_fr, accel_fr, decel_rate, jerk_now);
|
|
cmd_rl = jerkLimitedStep(cmd_rl, target_rl, accel_rl, decel_rate, jerk_now);
|
|
cmd_rr = jerkLimitedStep(cmd_rr, target_rr, accel_rr, 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);
|
|
// 스로틀 중립 복귀로 인한 자연 정지: 전자식 브레이크는 잠그지 않는다
|
|
// (CH5를 눌렀을 때만 잠금 — 위 브레이크 최우선 로직 참고).
|
|
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 << ',' << raw_steer << ','
|
|
<< raw_throttle << ',' << (rc_ok ? 1 : 0) << ','
|
|
<< (brake_on ? 1 : 0) << ',' << v_max_now << ',' << v_x << ','
|
|
<< omega << ',' << raw_ch[1] << ',' << raw_ch[2] << ','
|
|
<< raw_ch[3] << ',' << raw_ch[4] << ',' << raw_ch[5] << ','
|
|
<< raw_ch[6] << ',' << raw_ch[7] << ',' << raw_ch[8] << ','
|
|
<< 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';
|
|
}
|
|
|
|
// RC 조종기 입력값 피드백: 실기 조작 중 CH1~CH8 raw 값(CH5=브레이크 값
|
|
// 직접 확인용 포함)과 그로부터 계산된 선속도/각속도 지령을 실시간으로
|
|
// 확인할 수 있도록 HUD에 표시한다.
|
|
const char *speed_mode_str =
|
|
(v_max_now <= rc_cfg.v_max_low + 0.001f) ? "LOW " : "HIGH";
|
|
printf("\r\033[K[%-8s] RC:%s%s "
|
|
"C1:%4d C2:%4d C3:%4d C4:%4d C5:%4d C6:%4d C7:%4d C8:%4d "
|
|
"SPD:%s VX:%+4.2f OMG:%+4.2f | AXLE:%5.3fm | %-28s | "
|
|
"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(), rc_ok ? "OK" : "LOST", brake_on ? "[BRK]" : " ",
|
|
raw_ch[1], raw_ch[2], raw_ch[3], raw_ch[4], raw_ch[5], raw_ch[6],
|
|
raw_ch[7], raw_ch[8], speed_mode_str, v_x, omega,
|
|
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";
|
|
}
|
|
|
|
rc.stop();
|
|
|
|
std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
|
|
return 0;
|
|
} |