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

Add driver error code diagnostics and current limit query/set

Browse Source 
Reads 0x20A5/0x20A6 error codes alongside existing feedback poll to
diagnose overcurrent/overload alarms, and adds rated/max current
query and max current write helpers.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
This commit is contained in:
robin
2026-08-13 10:14:40 +09:00
parent 9f2ead2138
commit ae3f92f358
2 changed files with 139 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xbox_motor_control.cpp
+139 -17
View File
@@ -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<uint16_t> 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<uint32_t>(regs[0]) << 16) | regs[1];
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[2]) << 16) | regs[3];
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[4]);
int16_t vr = static_cast<int16_t>(regs[5]);
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[6]);
int16_t tr = static_cast<int16_t>(regs[7]);
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
// -----------------------------------------------------------------------------
@@ -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<uint16_t>(max_current_a * 10.0f);
driver_front.setMaxCurrent(new_max_01a);
if (driver_rear_ptr)
driver_rear_ptr->setMaxCurrent(new_max_01a);
std::cout << " - 최대전류를 " << max_current_a << "A로 낮춤(4바퀴 동일)\n";
}
}
std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 "
"또는 Ctrl+C, 라이다 토글: X 버튼)\n\n";
@@ -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(
xbox_motor_control_cpp
BIN
View File
Binary file not shown.