azmo/fori_zltech_motor_test
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

upload previous file

Browse Source
This commit is contained in:
robin
2026-08-07 13:48:39 +09:00
parent 7b801d2721
commit 4fbe036427
8 changed files with 706 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xbox_motor_control.cpp
+549
View File
@@ -0,0 +1,549 @@
#include <iostream>
#include <vector>
#include <string>
#include <chrono>
#include <thread>
#include <cmath>
#include <algorithm>
#include <csignal>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <SDL2/SDL.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));
}
return rx_bytes == 8;
}
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));
}
return rx_bytes == 8;
}
bool readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector<uint16_t>& out_vals) {
uint8_t pkt[8];
pkt[0] = slave;
pkt[1] = 0x03;
pkt[2] = (reg >> 8) & 0xFF;
pkt[3] = reg & 0xFF;
pkt[4] = (count >> 8) & 0xFF;
pkt[5] = count & 0xFF;
uint16_t crc = calcCRC16(pkt, 6);
pkt[6] = crc & 0xFF;
pkt[7] = (crc >> 8) & 0xFF;
tcflush(fd_, TCIOFLUSH);
if (write(fd_, pkt, 8) != 8) return false;
size_t expected_bytes = 5 + count * 2;
std::vector<uint8_t> rx_buf(expected_bytes);
size_t rx_bytes = 0;
auto start = std::chrono::steady_clock::now();
while (rx_bytes < expected_bytes) {
ssize_t res = read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
if (res > 0) rx_bytes += res;
auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 15) break;
std::this_thread::sleep_for(std::chrono::microseconds(100));
}
if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
out_vals.resize(count);
for (uint16_t i = 0; i < count; ++i) {
out_vals[i] = (static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
}
return true;
}
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) {
std::vector<uint16_t> regs;
if (port_->readRegs(slave_id_, 0x20A7, 6, regs)) {
uint32_t val_l = ((static_cast<uint32_t>(regs[0]) & 0xFFFF) << 16) | (regs[1] & 0xFFFF);
l_tick = (val_l < 0x80000000U) ? static_cast<int32_t>(val_l) : static_cast<int32_t>(val_l - 0x100000000ULL);
uint32_t val_r = ((static_cast<uint32_t>(regs[2]) & 0xFFFF) << 16) | (regs[3] & 0xFFFF);
r_tick = (val_r < 0x80000000U) ? static_cast<int32_t>(val_r) : static_cast<int32_t>(val_r - 0x100000000ULL);
int16_t vl = static_cast<int16_t>(regs[4]);
int16_t vr = static_cast<int16_t>(regs[5]);
l_fb = static_cast<float>(vl);
r_fb = static_cast<float>(vr);
return true;
}
return false;
}
};
float applyDeadzone(float val, float deadzone = 0.1f) {
if (std::abs(val) < deadzone) return 0.0f;
float sign = (val > 0.0f) ? 1.0f : -1.0f;
float norm = (std::abs(val) - deadzone) / (1.0f - deadzone);
// 선형 커브 (Linear Curve): 토크 저하 없이 스틱 조작에 100% 즉각 출력을 보장
return sign * norm;
}
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.30f; // m/s
float max_spin_v = 0.30f; // m/s
float track_width = 0.576f; // 좌우 바퀴 중심 거리 576mm
float wheelbase = 0.3684f; // 전후 바퀴 중심 거리 368.4mm
float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름 (131.517mm)
float accel_rate = 120.0f; // RPM/s
float decel_rate = 180.0f; // RPM/s (역기전력 서지 방지 및 소프트 정지 튜닝)
float bumper_offset = 0.045f; // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리 (45mm)
for (int i = 1; i < argc; ++i) {
std::string arg = argv[i];
if (arg == "--port" && i + 1 < argc) port1 = argv[++i];
else if (arg == "--port1" && i + 1 < argc) port1 = argv[++i];
else if (arg == "--port2" && i + 1 < argc) port2 = argv[++i];
else if (arg == "--id1" && i + 1 < argc) id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
else if (arg == "--id2" && i + 1 < argc) id2 = static_cast<uint8_t>(std::stoi(argv[++i]));
else if (arg == "--track_width" && i + 1 < argc) track_width = std::stof(argv[++i]);
else if (arg == "--wheelbase" && i + 1 < argc) wheelbase = std::stof(argv[++i]);
else if (arg == "--radius" && i + 1 < argc) wheel_radius = std::stof(argv[++i]);
else if (arg == "--bumper" && i + 1 < argc) bumper_offset = std::stof(argv[++i]);
else if (arg == "--accel" && i + 1 < argc) accel_rate = std::stof(argv[++i]);
else if (arg == "--decel" && i + 1 < argc) decel_rate = std::stof(argv[++i]);
else if (arg == "--bcast" || arg == "--broadcast") bcast_mode = true;
}
constexpr float PI_VAL = 3.14159265358979323846f;
float rpm_per_ms = 60.0f / (2.0f * PI_VAL * wheel_radius);
// 4WD 유효 선회 계수 (Track Width 576mm, Wheelbase 368.4mm 정밀 기하학)
float effective_w = std::sqrt(track_width * track_width + wheelbase * wheelbase);
float k_skid = (track_width + (wheelbase * wheelbase) / track_width) / 2.0f;
std::cout << "\n==================================================================\n";
std::cout << " [C++17 4WD 정밀 직진/감속 보정] ZLAC8015D 4모터 동기화 시스템\n";
std::cout << " - 좌우 바퀴 거리 (Track Width): " << track_width * 1000.0f << " mm\n";
std::cout << " - 전후 바퀴 거리 (Wheelbase) : " << wheelbase * 1000.0f << " mm\n";
std::cout << " - 범퍼 오프셋 (Bumper Offset) : " << bumper_offset * 1000.0f << " mm\n";
std::cout << " - 실시간 직진 엔코더 자동 수평 보정 (Straight Auto-Correction) 활성화\n";
std::cout << "==================================================================\n";
if (SDL_Init(SDL_INIT_JOYSTICK) < 0) {
std::cerr << "[오류] SDL 조이스틱 초기화 실패: " << SDL_GetError() << std::endl;
return 1;
}
if (SDL_NumJoysticks() < 1) {
std::cerr << "[경고] 무선 Xbox 컨트롤러(조이스틱)가 연결되지 않았습니다.\n";
SDL_Quit();
return 1;
}
SDL_Joystick* joystick = SDL_JoystickOpen(0);
if (!joystick) {
std::cerr << "[오류] 조이스틱 열기 실패!\n";
SDL_Quit();
return 1;
}
std::cout << "[정보] C++ Xbox 컨트롤러 연결 성공: " << SDL_JoystickName(joystick) << std::endl;
SerialPort sp1, sp2;
if (!sp1.openPort(port1)) return 1;
std::cout << "[정보] RS485 포트1 (" << port1 << ") 오픈 성공.\n";
SerialPort* sp2_ptr = &sp1;
if (!port2.empty()) {
if (sp2.openPort(port2)) {
sp2_ptr = &sp2;
std::cout << "[정보] RS485 포트2 (" << port2 << ") 오픈 성공.\n";
}
}
MotorDriver driver_front(&sp1, bcast_mode ? 0 : id1);
driver_front.initDriver(150, 150);
MotorDriver* driver_rear_ptr = nullptr;
MotorDriver driver_rear(sp2_ptr, bcast_mode ? 0 : id2);
if (!bcast_mode) {
driver_rear.initDriver(150, 150);
driver_rear_ptr = &driver_rear;
}
std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 또는 Ctrl+C)\n\n";
float target_fl = 0.0f, target_fr = 0.0f;
float target_rl = 0.0f, target_rr = 0.0f;
float cmd_fl = 0.0f, cmd_fr = 0.0f;
float cmd_rl = 0.0f, cmd_rr = 0.0f;
const float loop_hz = 100.0f;
const float dt = 1.0f / loop_hz;
const float max_accel_step = accel_rate * dt;
const float max_decel_step = decel_rate * dt;
std::string state = "STOPPED";
bool trip_initialized = false;
int32_t start_fl = 0, start_fr = 0, start_rl = 0, start_rr = 0;
// S-Curve 부드러운 가속 보정 람다 함수 (좌/우 모터 대칭 가속 보장)
auto sCurveStep = [](float current, float target, float max_accel, float max_decel) {
float diff = target - current;
if (std::abs(diff) < 0.01f) return target;
// 속도 절댓값 크기가 커지는 중이면 가속(accel), 줄어드는 중이면 감속(decel) 적용
bool is_accelerating = std::abs(target) > std::abs(current);
float step_rate = is_accelerating ? max_accel : max_decel;
float step = (diff > 0.0f) ? step_rate : -step_rate;
float ratio = std::clamp(std::abs(diff) / 60.0f, 0.15f, 1.0f);
float smooth = ratio * ratio * (3.0f - 2.0f * ratio);
float delta = step * smooth;
if (std::abs(delta) > std::abs(diff)) return target;
return current + delta;
};
while (g_running) {
SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_JOYBUTTONDOWN) {
if (event.jbutton.button == 0) { // A 버튼 누르면 거리 0m 리셋
trip_initialized = false;
} else if (event.jbutton.button == 1 || event.jbutton.button == 6) { // B or Back
g_running = 0;
}
}
}
Sint16 lx_raw = SDL_JoystickGetAxis(joystick, 0);
Sint16 ly_raw = SDL_JoystickGetAxis(joystick, 1);
Sint16 rx_raw = SDL_JoystickGetAxis(joystick, 3);
float ly = applyDeadzone(-static_cast<float>(ly_raw) / 32767.0f);
float lx = applyDeadzone(static_cast<float>(lx_raw) / 32767.0f);
float rx = applyDeadzone(static_cast<float>(rx_raw) / 32767.0f);
// 직진 제어 락 (Straight-Drive Lock): 스틱 좌우 15% 이내 기울임은 100% 칼직진 고정!
if (std::abs(lx) < 0.15f) {
lx = 0.0f;
}
if (std::abs(rx) < 0.15f) {
rx = 0.0f;
}
float v_x = ly * max_v; // 선속도 m/s
float omega = 0.0f; // 각속도 rad/s
if (std::abs(rx) > 0.0f) {
// 제자리 회전 (Spin Turn)
omega = -rx * (max_spin_v / (effective_w / 2.0f));
float v_l = -omega * (effective_w / 2.0f);
float v_r = +omega * (effective_w / 2.0f);
target_fl = v_l * rpm_per_ms;
target_fr = -v_r * rpm_per_ms;
target_rl = v_l * rpm_per_ms;
target_rr = -v_r * rpm_per_ms;
} else {
// 직진 및 커브 차동 선회 (Kinematics)
omega = -lx * (max_v / (effective_w / 2.0f));
float v_l = v_x - omega * k_skid;
float v_r = v_x + omega * k_skid;
target_fl = v_l * rpm_per_ms;
target_fr = -v_r * rpm_per_ms;
target_rl = v_l * rpm_per_ms;
target_rr = -v_r * rpm_per_ms;
}
if (state == "STOPPED") {
if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
driver_front.setBrakes(false);
if (driver_rear_ptr) driver_rear_ptr->setBrakes(false);
std::this_thread::sleep_for(std::chrono::milliseconds(50));
state = "RUNNING";
}
} else if (state == "RUNNING") {
cmd_fl = sCurveStep(cmd_fl, target_fl, max_accel_step, max_decel_step);
cmd_fr = sCurveStep(cmd_fr, target_fr, max_accel_step, max_decel_step);
cmd_rl = sCurveStep(cmd_rl, target_rl, max_accel_step, max_decel_step);
cmd_rr = sCurveStep(cmd_rr, target_rr, max_accel_step, max_decel_step);
if (std::abs(target_fl) < 0.1f && std::abs(target_fr) < 0.1f &&
std::abs(cmd_fl) < 0.5f && std::abs(cmd_fr) < 0.5f) {
state = "STOPPING";
}
} else if (state == "STOPPING") {
// 감속 시에는 즉각적으로 정지되도록 빠르게 감속 적용 (슬라이딩 감속 지연 제거)
cmd_fl = sCurveStep(cmd_fl, target_fl, max_decel_step, max_decel_step);
cmd_fr = sCurveStep(cmd_fr, target_fr, max_decel_step, max_decel_step);
cmd_rl = sCurveStep(cmd_rl, target_rl, max_decel_step, max_decel_step);
cmd_rr = sCurveStep(cmd_rr, target_rr, max_decel_step, max_decel_step);
if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) {
cmd_fl = 0.0f; cmd_fr = 0.0f;
cmd_rl = 0.0f; cmd_rr = 0.0f;
driver_front.setRPMs(0, 0);
if (driver_rear_ptr) driver_rear_ptr->setRPMs(0, 0);
std::this_thread::sleep_for(std::chrono::milliseconds(50));
driver_front.setBrakes(true);
if (driver_rear_ptr) driver_rear_ptr->setBrakes(true);
state = "STOPPED";
}
}
auto comm_start = std::chrono::high_resolution_clock::now();
if (state != "STOPPED") {
driver_front.setRPMs(cmd_fl, cmd_fr);
if (driver_rear_ptr) driver_rear_ptr->setRPMs(cmd_rl, cmd_rr);
}
float fl_fb = 0, fr_fb = 0, rl_fb = 0, rr_fb = 0;
int32_t fl_tick = 0, fr_tick = 0, rl_tick = 0, rr_tick = 0;
driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick);
if (driver_rear_ptr) driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick);
auto comm_end = std::chrono::high_resolution_clock::now();
float comm_ms = std::chrono::duration<float, std::milli>(comm_end - comm_start).count();
// 16384 Ticks = 4배 배율 4채널 엔코더 1회전 (0.798m) 정밀 반영
float meters_per_tick = (2.0f * PI_VAL * wheel_radius) / 16384.0f;
if (!trip_initialized && (fl_tick != 0 || fr_tick != 0)) {
start_fl = fl_tick; start_fr = fr_tick;
start_rl = rl_tick; start_rr = rr_tick;
trip_initialized = true;
}
int32_t delta_fl = std::abs(fl_tick - start_fl);
int32_t delta_fr = std::abs(fr_tick - start_fr);
int32_t delta_rl = std::abs(rl_tick - start_rl);
int32_t delta_rr = std::abs(rr_tick - start_rr);
float dist_fl = delta_fl * meters_per_tick;
float dist_fr = delta_fr * meters_per_tick;
float dist_rl = delta_rl * meters_per_tick;
float dist_rr = delta_rr * meters_per_tick;
float dist_axle = (dist_fl + dist_fr + dist_rl + dist_rr) / 4.0f;
float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);
printf("\r\033[K[%-8s] AXLE: %5.3fm | BUMPER: %5.3fm | Ticks: FL=%+5d FR=%+5d | LATENCY: %4.1fms",
state.c_str(), dist_axle, dist_bumper, delta_fl, delta_fr, comm_ms);
fflush(stdout);
std::this_thread::sleep_for(std::chrono::microseconds(static_cast<int>(dt * 1000000)));
}
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 (joystick) SDL_JoystickClose(joystick);
SDL_Quit();
std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
return 0;
}