diff --git a/xbox_motor_control.cpp b/xbox_motor_control.cpp index ffbf1ce..6324a94 100644 --- a/xbox_motor_control.cpp +++ b/xbox_motor_control.cpp @@ -312,37 +312,97 @@ public: } bool readFeedback(float &l_fb, float &r_fb, int32_t &l_tick, - int32_t &r_tick, float &l_torque_a, float &r_torque_a) { + int32_t &r_tick, float &l_torque_a, float &r_torque_a, + uint16_t &err_l, uint16_t &err_r) { std::vector regs; - // 0x20A7~0x20AE 8레지스터 연속 읽기: 포지션 틱, RPM 피드백에 이어 - // 실제 토크(전류, 0x20AD/0x20AE)까지 한 번의 통신으로 확보. - // 무부하(공중에 뜬) 바퀴는 전류가 급격히 낮아지므로 슬립 진단에 사용. - if (port_->readRegs(slave_id_, 0x20A7, 8, 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(regs[0]) << 16) | regs[1]; + uint32_t val_l = (static_cast(regs[2]) << 16) | regs[3]; l_tick = static_cast(val_l); - uint32_t val_r = (static_cast(regs[2]) << 16) | regs[3]; + uint32_t val_r = (static_cast(regs[4]) << 16) | regs[5]; r_tick = static_cast(val_r); - int16_t vl = static_cast(regs[4]); - int16_t vr = static_cast(regs[5]); + int16_t vl = static_cast(regs[6]); + int16_t vr = static_cast(regs[7]); l_fb = static_cast(vl) * 0.1f; // 0.1RPM 단위 -> RPM r_fb = static_cast(vr) * 0.1f; - int16_t tl = static_cast(regs[6]); - int16_t tr = static_cast(regs[7]); + int16_t tl = static_cast(regs[8]); + int16_t tr = static_cast(regs[9]); l_torque_a = static_cast(tl) * 0.1f; // 0.1A 단위 -> A r_torque_a = static_cast(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 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 // ----------------------------------------------------------------------------- @@ -587,6 +647,11 @@ int main(int argc, char **argv) { 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; @@ -647,6 +712,8 @@ int main(int argc, char **argv) { 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) @@ -721,6 +788,7 @@ int main(int argc, char **argv) { "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 { @@ -785,6 +853,40 @@ int main(int argc, char **argv) { 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(max_current_a * 10.0f); + driver_front.setMaxCurrent(new_max_01a); + if (driver_rear_ptr) + driver_rear_ptr->setMaxCurrent(new_max_01a); + std::cout << " - 최대전류를 " << max_current_a << "A로 낮춤(4바퀴 동일)\n"; + } + } + std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 " "또는 Ctrl+C, 라이다 토글: X 버튼)\n\n"; @@ -841,6 +943,7 @@ 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; char wheel_status_str[5] = "OOOO"; + bool fault_active = false; // 드라이버 알람(과전류/과부하 등) 발생 여부 while (g_running) { SDL_Event event; @@ -1060,11 +1163,28 @@ int main(int argc, char **argv) { 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); + 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); + 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 = @@ -1167,14 +1287,16 @@ int main(int argc, char **argv) { << rl_fb << ',' << rr_fb << ',' << fl_amp << ',' << fr_amp << ',' << rl_amp << ',' << rr_amp << ',' << wheel_status_str[0] << ',' << wheel_status_str[1] << ',' << wheel_status_str[2] - << ',' << wheel_status_str[3] << ',' << dist_axle << ',' - << comm_ms << '\n'; + << ',' << wheel_status_str[3] << ',' << err_f_l << ',' + << err_f_r << ',' << err_r_l << ',' << err_r_r << ',' + << dist_axle << ',' << comm_ms << '\n'; } printf("\r\033[K[%-8s] AXLE:%5.3fm | %-28s | I(A):FL%+4.1f FR%+4.1f " - "RL%+4.1f RR%+4.1f | WHL(FL/FR/RL/RR):%s | %4.1fms", + "RL%+4.1f RR%+4.1f | WHL(FL/FR/RL/RR):%s%s | %4.1fms", state.c_str(), dist_axle, lidar_telemetry.c_str(), fl_amp, fr_amp, - rl_amp, rr_amp, wheel_status_str, comm_ms); + rl_amp, rr_amp, wheel_status_str, + fault_active ? " [!!알람!!]" : "", comm_ms); fflush(stdout); std::this_thread::sleep_for( diff --git a/xbox_motor_control_cpp b/xbox_motor_control_cpp index 3df3056..8cdb1b8 100755 Binary files a/xbox_motor_control_cpp and b/xbox_motor_control_cpp differ