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

change the controler

Browse Source
This commit is contained in:
robin
2026-08-19 14:44:25 +09:00
parent 6be30eb929
commit dec1d85f9b
3 changed files with 128 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xbox_motor_control.cpp
+116 -46
View File
@@ -1,4 +1,5 @@
#include <algorithm>
#include <array>
#include <atomic>
#include <cerrno>
#include <chrono>
@@ -627,7 +628,7 @@ struct termios2 { // <asm/termbits.h>의 커널 ABI와 동일 레이아웃 (glib
};
struct RcConfig {
std::string port = "/dev/ttyUSB1";
std::string port = "/dev/ttyUSB0"; // 현재 고정 배선: RC 수신기(XR1) = ttyUSB0
int baud = 420000; // CRSF 표준 보드레이트
int ch_steer = 1; // CH1: 좌우 조향
@@ -646,6 +647,12 @@ struct RcConfig {
int brake_threshold = 1500; // 이상이면 브레이크 ON
int speed_threshold = 1500; // 이상이면 저속 모드
// CH1/CH2(연속 아날로그 스틱) 슬루레이트 한계: 메인 루프 한 틱(~10ms)
// 사이에 raw 값이 이 이상 튀면 사람이 스틱으로 낼 수 없는 변화로 보고
// 손상된 프레임으로 간주해 버린다. CH5/CH6(스위치)는 원래 한 번에 크게
// 뛰는 게 정상이라 이 제한을 적용하지 않는다.
int max_slew_per_tick = 200;
float v_max_low = 0.3f; // m/s (CH6 저속 모드)
float v_max_high = 1.5f; // m/s (CH6 고속 모드)
@@ -703,6 +710,22 @@ private:
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; // 스펙상 프레임은 훨씬 작음
@@ -808,7 +831,21 @@ private:
bit_count -= 11;
int us_val = static_cast<int>(
std::lround((raw_val - 172) * 1000.0 / 1639.0 + 1000.0));
parsed[ch + 1] = us_val; // CH1..CH16 (1-based)
// 미디언-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;
@@ -874,7 +911,7 @@ int main(int argc, char **argv) {
std::signal(SIGINT, signalHandler);
std::signal(SIGTERM, signalHandler);
std::string port1 = "/dev/ttyUSB0";
std::string port1 = "/dev/ttyUSB1"; // 현재 고정 배선: 모터 드라이버 = ttyUSB1
std::string port2 = "";
uint8_t id1 = 1;
uint8_t id2 = 2;
@@ -884,12 +921,12 @@ int main(int argc, char **argv) {
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 (최대 가속도 한계)
// 가속은 역기전력 보호회로가 하드웨어로 구성되어 목표값에 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 (제자리 회전 저크 한계: 고마찰 노면에서
// 정지마찰이 급격히 파괴되지 않도록 accel_rate에
// 도달하는 시간을 더 길게 늘림)
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을 로봇 중심에서
// 살짝 벗어나게 해 스크럽 마찰 저항을 줄임)
@@ -951,8 +988,6 @@ int main(int argc, char **argv) {
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)
@@ -1031,6 +1066,8 @@ int main(int argc, char **argv) {
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_vmax_low" && i + 1 < argc)
rc_cfg.v_max_low = std::stof(argv[++i]);
else if (arg == "--rc_vmax_high" && i + 1 < argc)
@@ -1064,7 +1101,7 @@ int main(int argc, char **argv) {
<< "mm\n";
std::cout << " - 최고 속도: 저속 " << rc_cfg.v_max_low << "m/s / 고속 "
<< rc_cfg.v_max_high << "m/s (CH" << rc_cfg.ch_speed
<< " 전환) | 가속: " << accel_rate << "RPM/s | Jerk: " << jerk_rate
<< " 전환) | 가속: 즉시 추종(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
@@ -1208,13 +1245,14 @@ int main(int argc, char **argv) {
const float loop_hz = 100.0f;
const float dt = 1.0f / loop_hz;
// 저크 제한(jerk-limited) S-curve: 가속도 자체를 매 틱 jerk_limit 만큼만
// 변화시켜 서서히 목표 가속도에 도달하게 함. 기존 구현은 목표 접근 구간만
// 부드러웠고 정지 상태에서 출발할 때는 첫 틱부터 거의 풀가속도가 걸려
// 정지마찰이 급격히 파괴되는 문제가 있었음.
// 가속: 역기전력 보호회로가 하드웨어로 구성되어 더 이상 소프트웨어로
// 완만하게 램프업할 필요가 없어져, 조이스틱 목표값에 1:1로 즉시 추종한다.
// 감속: 기존과 동일하게 저크 제한(jerk-limited) S-curve 유지 — 가속도
// 자체를 매 틱 jerk_limit 만큼만 변화시켜 서서히 목표 가속도에 도달하게
// 해서, 급감속 시 타이어 스크럽/전복 위험을 줄인다.
auto jerkLimitedStep = [dt](float current_vel, float target_vel,
float &current_accel, float max_accel_mag,
float max_decel_mag, float jerk_limit) {
float &current_accel, float max_decel_mag,
float jerk_limit) {
float diff = target_vel - current_vel;
if (std::abs(diff) < 0.01f) {
current_accel = 0.0f;
@@ -1222,9 +1260,13 @@ int main(int argc, char **argv) {
}
bool is_accelerating = std::abs(target_vel) > std::abs(current_vel);
float accel_cap = is_accelerating ? max_accel_mag : max_decel_mag;
if (is_accelerating) {
current_accel = 0.0f;
return target_vel;
}
// 이번 틱에 diff를 없애는데 필요한 가속도(부호 포함)를 물리적 한계로 클램프
float desired_accel = std::clamp(diff / dt, -accel_cap, accel_cap);
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,
@@ -1249,9 +1291,9 @@ int main(int argc, char **argv) {
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으로
// 스냅시켜 지연을 없앤다.
// 있는데 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";
@@ -1260,6 +1302,18 @@ int main(int argc, char **argv) {
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 같은 순간 점프)
// 두 경우 모두 그 틱은 버리고 마지막으로 유효했던 값을 그대로 사용한다.
int last_raw_steer = rc_cfg.steer_center_lo;
int last_raw_throttle = 1500;
bool steer_initialized = false, throttle_initialized = false;
while (g_running) {
// ---------------------------------------------------------------------
@@ -1273,6 +1327,30 @@ int main(int argc, char **argv) {
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;
bool steer_slew_ok = !steer_initialized ||
std::abs(raw_steer - last_raw_steer) <=
rc_cfg.max_slew_per_tick;
if (steer_range_ok && steer_slew_ok) {
last_raw_steer = raw_steer;
steer_initialized = true;
} else {
raw_steer = last_raw_steer; // 범위 밖 또는 비정상 점프 -> 손상된 프레임으로 간주
}
bool throttle_range_ok = raw_throttle >= rc_cfg.throttle_min &&
raw_throttle <= rc_cfg.throttle_max;
bool throttle_slew_ok = !throttle_initialized ||
std::abs(raw_throttle - last_raw_throttle) <=
rc_cfg.max_slew_per_tick;
if (throttle_range_ok && throttle_slew_ok) {
last_raw_throttle = raw_throttle;
throttle_initialized = true;
} else {
raw_throttle = last_raw_throttle;
}
// CH1~CH8 raw 값 전체를 확인용으로 읽는다 (CH5=브레이크 값을 실시간으로
// 직접 눈으로 검증할 수 있도록 HUD/CSV에 그대로 노출).
int raw_ch[9]; // 인덱스 1..8 사용
@@ -1442,6 +1520,8 @@ int main(int argc, char **argv) {
// 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;
@@ -1453,6 +1533,10 @@ int main(int argc, char **argv) {
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;
@@ -1460,10 +1544,6 @@ int main(int argc, char **argv) {
// 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) {
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") {
@@ -1491,14 +1571,10 @@ int main(int argc, char **argv) {
}
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);
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) {
@@ -1506,14 +1582,10 @@ int main(int argc, char **argv) {
}
} 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);
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;
@@ -1524,10 +1596,8 @@ int main(int argc, char **argv) {
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);
// 스로틀 중립 복귀로 인한 자연 정지: 전자식 브레이크는 잠그지 않는다
// (CH5를 눌렀을 때만 잠금 — 위 브레이크 최우선 로직 참고).
state = "STOPPED";
}
}