120 lines
4.2 KiB
C++
120 lines
4.2 KiB
C++
/**
|
|
* @file hand_setid.cpp
|
|
* @brief Give a hand a distinct RS-485 id, permanently, so left/right can never swap again.
|
|
*
|
|
* WHY THIS EXISTS
|
|
* The RH56 has no handedness register — it cannot tell you whether it is a left or a right
|
|
* unit. Both USB-RS485 dongles are CH340 1a86:7523 reporting the SAME USB serial. So the
|
|
* only thing distinguishing the two hands was the ttyUSB number, which the kernel hands out
|
|
* in enumeration order — and that changes across replugs, reboots and hub hiccups. When it
|
|
* flips, left and right silently trade places: nothing errors, and you drive the wrong hand.
|
|
* (Observed on this robot 2026-07-30: software "L.*" was in fact the robot's RIGHT hand.)
|
|
*
|
|
* Giving the two hands DIFFERENT ids fixes that at the root. inspire_g1 scans ids 1..4 on
|
|
* each port and, when the ids differ, uses them as identity: id 1 = RIGHT, id 2 = LEFT.
|
|
* That is wiring-independent and survives any replug.
|
|
*
|
|
* USAGE — run ONCE per robot, with the OTHER hand's dongle unplugged or on its own port:
|
|
* g++ -std=c++17 -Iinclude example/hand_setid.cpp -o build/hand_setid
|
|
* ./build/hand_setid /dev/ttyUSBx # just report which id answers
|
|
* ./build/hand_setid /dev/ttyUSBx 1 2 # change id 1 -> 2 and SAVE it
|
|
*
|
|
* Convention: leave the RIGHT hand at id 1, give the LEFT hand id 2.
|
|
* Needs the port free — stop the container first.
|
|
*
|
|
* Registers: HAND_ID 1000 (0x03E8), SAVE 1005 (0x03ED). The id change only survives a
|
|
* power cycle if SAVE is written afterwards, which this does.
|
|
*/
|
|
#include "inspire.h"
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <string>
|
|
#include <memory>
|
|
#include <unistd.h>
|
|
|
|
static uint8_t cks(const uint8_t *d, int len)
|
|
{
|
|
uint8_t s = 0;
|
|
for (int i = 2; i < len - 1; i++) s += d[i];
|
|
return s;
|
|
}
|
|
|
|
// Write the SAVE register (1005) so the new id survives a power cycle.
|
|
static void saveParams(SerialPort &sp, uint8_t id)
|
|
{
|
|
uint8_t c[9] = {0xEB, 0x90, id, 0x04, 0x12, 0xED, 0x03, 0x01, 0x00};
|
|
c[8] = cks(c, 9);
|
|
sp.send(c, 9);
|
|
usleep(300000);
|
|
uint8_t b[32];
|
|
sp.recv(b, sizeof(b));
|
|
}
|
|
|
|
// Which id answers on this port? -1 if none.
|
|
static int findId(SerialPort::SharedPtr sp)
|
|
{
|
|
for (int id = 1; id <= 4; id++)
|
|
{
|
|
inspire::InspireHand h(sp, (uint8_t)id);
|
|
Eigen::Matrix<double, 6, 1> q;
|
|
for (int t = 0; t < 3; t++)
|
|
if (h.GetPosition(q) == 0) return id;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
if (argc < 2)
|
|
{
|
|
printf("usage: %s <port> [from_id to_id]\n"
|
|
" %s /dev/ttyUSB0 report the id that answers\n"
|
|
" %s /dev/ttyUSB0 1 2 change id 1 -> 2 and save\n",
|
|
argv[0], argv[0], argv[0]);
|
|
return 1;
|
|
}
|
|
const char *dev = argv[1];
|
|
auto sp = std::make_shared<SerialPort>(dev, B115200, 8);
|
|
|
|
int cur = findId(sp);
|
|
if (cur < 0)
|
|
{
|
|
printf("%s: no hand answered on ids 1-4.\n"
|
|
" Is the container still holding the port? Is the hand powered?\n", dev);
|
|
return 2;
|
|
}
|
|
printf("%s: hand answers at rs485 id %d\n", dev, cur);
|
|
|
|
if (argc < 4) return 0;
|
|
|
|
int from = atoi(argv[2]), to = atoi(argv[3]);
|
|
if (from != cur)
|
|
{
|
|
printf("refusing: you said from=%d but this hand is at id %d.\n", from, cur);
|
|
return 3;
|
|
}
|
|
if (to < 1 || to > 4)
|
|
{
|
|
printf("refusing: to_id must be 1-4.\n");
|
|
return 3;
|
|
}
|
|
|
|
printf("changing id %d -> %d ...\n", from, to);
|
|
inspire::InspireHand h(sp, (uint8_t)from);
|
|
h.ChangeID((uint8_t)from, (uint8_t)to);
|
|
usleep(300000);
|
|
saveParams(*sp, (uint8_t)to);
|
|
usleep(300000);
|
|
|
|
int now = findId(sp);
|
|
if (now == to)
|
|
printf("OK — this hand is now id %d and it is saved.\n"
|
|
" Convention: RIGHT hand stays id 1, LEFT hand becomes id 2.\n"
|
|
" inspire_g1 will now identify the hands by id, and left/right can no longer\n"
|
|
" swap however the USB ports enumerate.\n", now);
|
|
else
|
|
printf("FAILED — hand now answers at id %d (wanted %d). Nothing saved reliably;\n"
|
|
" power-cycle the hand and re-check with: %s %s\n", now, to, argv[0], dev);
|
|
return now == to ? 0 : 4;
|
|
}
|