58 lines
2.0 KiB
C++
58 lines
2.0 KiB
C++
/**
|
|
* @file hand_wrtest.cpp
|
|
* @brief Reproduce inspire_g1's write-then-read cycle standalone (no DDS) to
|
|
* debug why GetPosition reads 0 inside the container but works natively.
|
|
*
|
|
* g++ -std=c++17 -Iinclude example/hand_wrtest.cpp -o /tmp/hand_wrtest
|
|
* /tmp/hand_wrtest [rightPort=/dev/ttyUSB1] [leftPort=/dev/ttyUSB0]
|
|
*
|
|
* Needs the ports free (stop inspire_g1 / the container first).
|
|
*/
|
|
#include "inspire.h"
|
|
#include <cstdio>
|
|
#include <memory>
|
|
#include <unistd.h>
|
|
|
|
using namespace inspire;
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
const char *pr = argc > 1 ? argv[1] : "/dev/ttyUSB1"; // right
|
|
const char *pl = argc > 2 ? argv[2] : "/dev/ttyUSB0"; // left
|
|
|
|
auto s1 = std::make_shared<SerialPort>(pr, B115200, 6);
|
|
auto s2 = std::make_shared<SerialPort>(pl, B115200, 6);
|
|
InspireHand right(s1, 1), left(s2, 1);
|
|
|
|
for (auto *h : {&right, &left})
|
|
{
|
|
h->ClearError();
|
|
h->SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
|
|
h->SetForce(500, 500, 500, 500, 500, 500);
|
|
}
|
|
|
|
// TIGHT loop, no inter-cycle delay, alternating open/close — the worst case
|
|
// that failed 300/300 with the old one-shot GetPosition. Verify the re-query
|
|
// GetPosition holds up.
|
|
Eigen::Matrix<double, 6, 1> qcmd, qr, ql;
|
|
int rfail = 0, lfail = 0, N = 300;
|
|
for (int cyc = 0; cyc < N; cyc++)
|
|
{
|
|
double v = ((cyc / 30) % 2 == 0) ? 1.0 : 0.0; // flip every 30 cycles
|
|
for (int i = 0; i < 6; i++) qcmd(i) = v;
|
|
right.SetPosition(qcmd);
|
|
left.SetPosition(qcmd);
|
|
usleep(3000);
|
|
int rr = right.GetPosition(qr);
|
|
usleep(2000);
|
|
int rl = left.GetPosition(ql);
|
|
if (rr != 0) rfail++;
|
|
if (rl != 0) lfail++;
|
|
if (cyc % 30 == 0)
|
|
printf("cyc%3d v=%.0f R(ret%d): %.2f %.2f %.2f L(ret%d): %.2f %.2f %.2f\n",
|
|
cyc, v, rr, qr(0), qr(1), qr(2), rl, ql(0), ql(1), ql(2));
|
|
}
|
|
printf("=== %d tight cycles: RIGHT read-fails=%d LEFT read-fails=%d ===\n", N, rfail, lfail);
|
|
return 0;
|
|
}
|