291 lines
11 KiB
C++
291 lines
11 KiB
C++
/**
|
|
* @file hand_diag.cpp
|
|
* @brief Read the Inspire RH56 fault registers per finger: ERROR, STATUS, TEMP,
|
|
* CURRENT. Tells you exactly WHY a finger won't move (locked rotor,
|
|
* over-temperature, over-current, ...) instead of guessing.
|
|
*
|
|
* Register map (from the Inspire Modbus map; this serial protocol encodes the
|
|
* address as low,high in bytes 5,6 — verified: CLEAR_ERROR 1004 = 0x03EC = {0xEC,0x03}):
|
|
* CURRENT 1594 (0x063A, 12 bytes) ERROR 1606 (0x0646, 6)
|
|
* STATUS 1612 (0x064C, 6) TEMP 1618 (0x0652, 6)
|
|
* CLEAR_ERROR 1004 (0x03EC) RESET_PARA 1006 (0x03EE)
|
|
*
|
|
* g++ -std=c++17 -Iinclude example/hand_diag.cpp -o /tmp/hand_diag
|
|
* /tmp/hand_diag /dev/ttyUSB1 [clear] # 'clear' also sends CLEAR_ERROR
|
|
*
|
|
* Needs the port free (stop the container first).
|
|
*/
|
|
#include "SerialPort.h"
|
|
#include <cstdio>
|
|
#include <cstdint>
|
|
#include <cstring>
|
|
#include <string>
|
|
#include <memory>
|
|
#include <unistd.h>
|
|
|
|
static const char *FING[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
|
|
|
|
static uint8_t cksum(const uint8_t *d, int len)
|
|
{
|
|
uint8_t s = 0;
|
|
for (int i = 2; i < len - 1; i++) s += d[i];
|
|
return s;
|
|
}
|
|
|
|
// Read `n` bytes from register `addr`, robustly (flush + re-query).
|
|
static bool readReg(SerialPort &sp, uint16_t addr, uint8_t n, uint8_t *out, uint8_t id = 1)
|
|
{
|
|
uint8_t q[9] = {0xEB, 0x90, id, 0x04, 0x11,
|
|
(uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8), n, 0x00};
|
|
q[8] = cksum(q, 9);
|
|
uint8_t buf[128];
|
|
for (int att = 0; att < 8; att++)
|
|
{
|
|
sp.flush();
|
|
sp.send(q, 9);
|
|
usleep(8000);
|
|
size_t got = sp.recv(buf, sizeof(buf));
|
|
for (size_t off = 0; off + 7 + n <= got; off++)
|
|
{
|
|
if (!((buf[off] == 0x90 && buf[off + 1] == 0xEB) ||
|
|
(buf[off] == 0xEB && buf[off + 1] == 0x90))) continue;
|
|
if (buf[off + 4] != 0x11) continue;
|
|
uint16_t got_addr = buf[off + 5] | (buf[off + 6] << 8);
|
|
if (got_addr != addr) continue;
|
|
int frame = 7 + n + 1; // header+addr+data+checksum
|
|
if (off + frame > got) continue;
|
|
if (buf[off + frame - 1] != cksum(buf + off, frame)) continue;
|
|
memcpy(out, buf + off + 7, n);
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static void writeReg16(SerialPort &sp, uint16_t addr, uint16_t val, uint8_t id = 1)
|
|
{
|
|
uint8_t c[11] = {0xEB, 0x90, id, 0x06, 0x12,
|
|
(uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8),
|
|
(uint8_t)(val & 0xFF), (uint8_t)(val >> 8), 0x00, 0x00};
|
|
c[9] = cksum(c, 10);
|
|
sp.send(c, 10);
|
|
usleep(5000);
|
|
uint8_t b[32];
|
|
sp.recv(b, sizeof(b));
|
|
}
|
|
|
|
static std::string errStr(uint8_t e)
|
|
{
|
|
if (!e) return "OK";
|
|
std::string s;
|
|
if (e & 0x01) s += "LOCKED_ROTOR ";
|
|
if (e & 0x02) s += "OVER_TEMPERATURE ";
|
|
if (e & 0x04) s += "OVER_CURRENT ";
|
|
if (e & 0x08) s += "ABNORMAL_OPERATION ";
|
|
if (e & 0x10) s += "COMMUNICATION_ERROR ";
|
|
return s;
|
|
}
|
|
|
|
// Command all 6 fingers to a position (ANGLE_SET 1486 = 0x05CE, 12 bytes).
|
|
static void setAll(SerialPort &sp, int16_t val, uint8_t id = 1)
|
|
{
|
|
uint8_t c[20];
|
|
c[0] = 0xEB; c[1] = 0x90; c[2] = id; c[3] = 0x0F; c[4] = 0x12;
|
|
c[5] = 0xCE; c[6] = 0x05;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
c[7 + 2 * i] = val & 0xFF;
|
|
c[8 + 2 * i] = (val >> 8) & 0xFF;
|
|
}
|
|
c[19] = cksum(c, 20);
|
|
sp.send(c, 20);
|
|
usleep(5000);
|
|
uint8_t b[32];
|
|
sp.recv(b, sizeof(b));
|
|
}
|
|
|
|
// Write all 6 force limits (FORCE_SET 1498 = 0x05DA, 12 bytes).
|
|
static void setForceAll(SerialPort &sp, uint16_t val, uint8_t id = 1)
|
|
{
|
|
uint8_t c[20];
|
|
c[0] = 0xEB; c[1] = 0x90; c[2] = id; c[3] = 0x0F; c[4] = 0x12;
|
|
c[5] = 0xDA; c[6] = 0x05;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
c[7 + 2 * i] = val & 0xFF;
|
|
c[8 + 2 * i] = (val >> 8) & 0xFF;
|
|
}
|
|
c[19] = cksum(c, 20);
|
|
sp.send(c, 20);
|
|
usleep(5000);
|
|
uint8_t b[32];
|
|
sp.recv(b, sizeof(b));
|
|
}
|
|
|
|
// Does the motor actually energize when told to move? current>0 => it's trying
|
|
// (mechanically blocked); current==0 => it never energizes.
|
|
//
|
|
// IMPORTANT: current==0 alone does NOT mean the actuator is dead. The RH56 stops driving a
|
|
// finger the instant its MEASURED force (FORCE_ACT 1582) reaches its force LIMIT
|
|
// (FORCE_SET 1498), so a finger whose force-sensor zero has drifted above the limit draws
|
|
// 0 mA and is indistinguishable from a dead motor. This test used to inherit whatever limit
|
|
// the service last wrote (500g) and so reproduced exactly that failure while looking like an
|
|
// independent raw-serial check — it is how the G1's right mid+index were wrongly called dead
|
|
// when they in fact run their full range at a 1000g limit. It now sweeps the limit itself.
|
|
static void moveTest(SerialPort &sp, uint16_t force)
|
|
{
|
|
printf("\n=== MOVE TEST: force limit %ug, command CLOSE, watch current + force + angle ===\n",
|
|
force);
|
|
setForceAll(sp, force);
|
|
setAll(sp, 1000); // open first
|
|
usleep(800000);
|
|
|
|
uint8_t f0[12] = {0};
|
|
if (readReg(sp, 1582, 12, f0))
|
|
{
|
|
printf("resting force vs the %ug limit (>= limit => the driver refuses to move it):\n ",
|
|
force);
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
int16_t F = (int16_t)(f0[2 * i] | (f0[2 * i + 1] << 8));
|
|
printf("%s=%dg%s ", FING[i], F, (F >= (int)force || -F >= (int)force) ? "<<GATED" : "");
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
setAll(sp, 0); // CLOSE
|
|
for (int k = 0; k < 6; k++)
|
|
{
|
|
usleep(400000);
|
|
uint8_t ang[12] = {0}, cur[12] = {0}, sta[6] = {0};
|
|
bool ra = readReg(sp, 1546, 12, ang);
|
|
bool rc = readReg(sp, 1594, 12, cur);
|
|
bool rs = readReg(sp, 1612, 6, sta);
|
|
printf("t=%.1fs reads: angle=%s current=%s status=%s\n",
|
|
k * 0.4, ra ? "ok" : "FAIL", rc ? "ok" : "FAIL", rs ? "ok" : "FAIL");
|
|
if (!(ra || rc || rs)) continue;
|
|
printf(" ");
|
|
for (int i = 0; i < 6; i++) // ALL fingers: healthy ones are the control
|
|
{
|
|
uint16_t A = ang[2 * i] | (ang[2 * i + 1] << 8);
|
|
int16_t C = (int16_t)(cur[2 * i] | (cur[2 * i + 1] << 8));
|
|
printf("%s:a=%u,i=%d,s=%d ", FING[i], A, C, sta[i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
setAll(sp, 1000);
|
|
usleep(500000);
|
|
setForceAll(sp, 500); // restore the service default
|
|
printf("\nIf a finger stayed put at 500g but moves here, it is NOT dead — its force-sensor\n"
|
|
"zero has drifted. Re-zero the sensor with the hand unloaded (FORCE_SENSOR_CALIB),\n"
|
|
"or keep its limit above the offset. Compare: '%s move' vs '%s move 1000'.\n",
|
|
"hand_diag <port>", "hand_diag <port>");
|
|
}
|
|
|
|
// Are the dead channels merely CONFIGURED not to drive? A speed / force / current
|
|
// limit of 0 would give exactly the observed signature: no motion, no current, no
|
|
// error, status "reached target". That would be software-fixable.
|
|
static void paramDump(SerialPort &sp)
|
|
{
|
|
struct { const char *name; uint16_t addr; uint8_t bytes; } regs[] = {
|
|
{"ANGLE_SET ", 1486, 12},
|
|
{"FORCE_SET ", 1498, 12},
|
|
{"SPEED_SET ", 1522, 12},
|
|
{"CURRENT_LIM ", 1020, 12},
|
|
{"ANGLE_ACT ", 1546, 12},
|
|
};
|
|
printf("%-13s", "register");
|
|
for (int i = 0; i < 6; i++) printf("%9s", FING[i]);
|
|
printf("\n");
|
|
for (auto &r : regs)
|
|
{
|
|
uint8_t b[16] = {0};
|
|
bool ok = readReg(sp, r.addr, r.bytes, b);
|
|
printf("%-13s", r.name);
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
if (!ok) { printf("%9s", "FAIL"); continue; }
|
|
printf("%9d", (int16_t)(b[2 * i] | (b[2 * i + 1] << 8)));
|
|
}
|
|
printf("\n");
|
|
}
|
|
printf("\n(a 0 in SPEED_SET / FORCE_SET / CURRENT_LIM for a finger would explain\n"
|
|
" no motion + no current + no error, and would be software-fixable.)\n");
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
const char *dev = argc > 1 ? argv[1] : "/dev/ttyUSB1";
|
|
std::string opt = argc > 2 ? argv[2] : "";
|
|
bool doClear = (opt == "clear");
|
|
bool doMove = (opt == "move");
|
|
SerialPort sp(dev, B115200, 8);
|
|
if (opt == "params") { paramDump(sp); return 0; }
|
|
// Command a CLOSE, then read ANGLE_SET back: does the setpoint actually LAND in
|
|
// the dead channels? ANGLE_SET=0 but no motion => setpoint accepted, motor dead.
|
|
// ANGLE_SET stays 1000 => the channel rejects the write.
|
|
if (opt == "setclose")
|
|
{
|
|
printf("commanding CLOSE (ANGLE_SET=0) ...\n\n");
|
|
setAll(sp, 0);
|
|
usleep(1500000);
|
|
paramDump(sp);
|
|
setAll(sp, 1000);
|
|
return 0;
|
|
}
|
|
// 'move [force]' — force defaults to the RH56 max so a drifted sensor zero can't
|
|
// silently gate a healthy finger off and make it look dead.
|
|
if (doMove)
|
|
{
|
|
uint16_t f = argc > 3 ? (uint16_t)atoi(argv[3]) : 1000;
|
|
moveTest(sp, f);
|
|
return 0;
|
|
}
|
|
|
|
if (doClear)
|
|
{
|
|
printf("sending CLEAR_ERROR (1004) ...\n");
|
|
writeReg16(sp, 1004, 1);
|
|
usleep(300000);
|
|
}
|
|
|
|
uint8_t err[6] = {0}, sta[6] = {0}, tmp[6] = {0}, cur[12] = {0};
|
|
uint8_t fact[12] = {0}, fset[12] = {0};
|
|
bool eok = readReg(sp, 1606, 6, err);
|
|
bool sok = readReg(sp, 1612, 6, sta);
|
|
bool tok = readReg(sp, 1618, 6, tmp);
|
|
bool cok = readReg(sp, 1594, 12, cur);
|
|
bool faok = readReg(sp, 1582, 12, fact); // FORCE_ACT — measured
|
|
bool fsok = readReg(sp, 1498, 12, fset); // FORCE_SET — the limit
|
|
|
|
printf("port %s (error=%s status=%s temp=%s current=%s force=%s limit=%s)\n\n", dev,
|
|
eok ? "ok" : "FAIL", sok ? "ok" : "FAIL", tok ? "ok" : "FAIL", cok ? "ok" : "FAIL",
|
|
faok ? "ok" : "FAIL", fsok ? "ok" : "FAIL");
|
|
printf("%-8s %6s %8s %6s %9s %8s %7s %s\n",
|
|
"finger", "err", "status", "temp", "current", "force", "limit", "meaning");
|
|
bool anyGated = false;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
int16_t c = cok ? (int16_t)(cur[2 * i] | (cur[2 * i + 1] << 8)) : 0;
|
|
int16_t fa = faok ? (int16_t)(fact[2 * i] | (fact[2 * i + 1] << 8)) : 0;
|
|
int16_t fs = fsok ? (int16_t)(fset[2 * i] | (fset[2 * i + 1] << 8)) : 0;
|
|
// THE check that was missing: a finger already at/over its force limit is gated off
|
|
// by the driver and will read 0 current with no error — identical to a dead motor.
|
|
bool gated = faok && fsok && fs > 0 && (fa >= fs || -fa >= fs);
|
|
if (gated) anyGated = true;
|
|
std::string mean = eok ? errStr(err[i]) : "?";
|
|
if (gated) mean = "FORCE-GATED (not dead: force >= limit)";
|
|
printf("%-8s 0x%02X %8d %5d\xC2\xB0 %8d %7dg %6dg %s\n",
|
|
FING[i], eok ? err[i] : 0, sok ? sta[i] : -1, tok ? tmp[i] : -1, c,
|
|
fa, fs, mean.c_str());
|
|
}
|
|
if (anyGated)
|
|
printf("\n>> One or more fingers is FORCE-GATED: its measured force already meets its\n"
|
|
">> limit, so the driver refuses to energise it (0 current, no error). This is a\n"
|
|
">> drifted force-sensor zero, NOT a dead actuator. Confirm with:\n"
|
|
">> %s %s move 1000\n"
|
|
">> Fix by re-zeroing the sensor with the hand unloaded, or raise the limit.\n",
|
|
argv[0], dev);
|
|
return 0;
|
|
}
|