Lynxmotion SES V2 Hexapod Robot

[cyberpalin]

@kurte

@dialfonzo @cbenson Sounds good, will update soon. Probably tomorrow… Mostly taking today off.

Same here. Probably will work it tomorrow. Been busy with normal life stuff today.

[cyberpalin]

@dialfonzo

@kurte - Everyone need a bit of time to relax. I wanted to make sure the firmware was ready for you (all) to test it in the weekend if needed.

Hope this gets you before you head home for the weekend.

Decided not to wait for tomorrow to do some preliminary tests. So here goes.

1. Update to new firmware for the ST1’s went without a hitch. Used the Restore Settings and after update the settings were restored including the baudrate. Then set baud 921600 for testing.
2. Update to new firmware for HS1 had a bit of hitch. Settings were not restored manually reset them and then set baud to 921600.
3. Using @kurte’s test_servos sketch, option i, the one that prints errors if the servos are updated. This time round no errors were noted:

Cmd: i
<24000, 165 228> 
Checks Status wait loops 817 in us: 504819

Print Servo Positions Joint(Goal, timed, end)
C:2(0, 1, -1)	F:4(-900, -206, -628)	T:6(-600, -133, -569)
<1440, 320 480> 
Checks Status wait loops 619 in us: 383276

Print Servo Positions Joint(Goal, timed, end)
C:2(100, 116, 113)	F:4(-450, -729, -477)	T:6(-300, -442, -303)
<1440, 320 480> 
Checks Status wait loops 1 in us: 3173

Print Servo Positions Joint(Goal, timed, end)
C:2(200, 100, 113)	F:4(0, -450, -438)	T:6(0, -300, -303)
<720, 320 320> 
Checks Status wait loops 432 in us: 382327

Print Servo Positions Joint(Goal, timed, end)
C:2(0, 6, -8)	F:4(450, 162, 396)	T:6(450, 184, 426)
<1440, 320 320> 
Checks Status wait loops 295 in us: 182824

Print Servo Positions Joint(Goal, timed, end)
C:2(-100, 0, -68)	F:4(0, 450, 387)	T:6(0, 450, 375)
<1440, 320 480> 
Checks Status wait loops 484 in us: 392036

Print Servo Positions Joint(Goal, timed, end)
C:2(-200, -211, -200)	F:4(-450, -176, -424)	T:6(-300, -174, -298)
<720, 320 480> 
Checks Status wait loops 345 in us: 324239

Print Servo Positions Joint(Goal, timed, end)
C:2(0, 6, 18)	F:4(-900, -622, -773)	T:6(-600, -467, -566)

Again this is all preliminary. Will put a LA on it when I get time tomorrow. So looking good so far.

[kurte]

Thanks @cyberpalin will probably wait until tomorrow… But good to know! Will be interesting to try the 8 and 9 commands and see how the timings compared for query times. Maybe also enable the properties I commented out…

I may probably first try at 250kb as that is what they run at now. Be interesting to see the query speedup… Then will probably convert to 750k speed, I am pretty sure there should be 0% off… I have test sketch in the project, which will change the servos baud rate…

Again looking promising

[kurte]

@dialfonzo @cyberpalin - I updated all 20 of my servos (18 in hex - 2 I purchased to test with)…
Nice utility… Wish it was faster

I started off running my test program with the servo query functions. To see if I noticed anything failing and curious about speed ups… I left the servos configured for 250K

Before this the last time I posted the command 9 1 and 8 1 the timings were something like:
27921 for (9 1 does individual query functions)
26959 for (8 1 output one long query and wait for results)

Ran it after servos updated: on servos 1 and 2.
9 1 Total Time: 29175
8 1 Total Time: 20254
9 2 Total Time: 28752
9 2 Total Time: 20335

So individual query is about same speed maybe slower
but output all queries one string: did speed up

Cmd: 9 1

Servo 1 values
  Q(-1) - 1 t:778
  QO(-1) - 0 t:808
  QAR(-1) - 1800 t:1120
  QP(-1) - 1468 t:1009
  QD(-1) - -29 t:952
  QWD(-1) - 4 t:968
  QWR(-1) - 0 t:923
  QS(-1) - 0 t:821
  QSD(-1) - 600 t:1053
  QSR(-1) - 100 t:1060
  QLED(-1) - 0 t:1023
  QG(-1) - 1 t:895
  QID(-1) - 1 t:925
  QB(-1) - -12144 t:1524
  QFD(-1) - DIS t:1783
  QMS(-1) - LSS-HS1 t:1181
  QF(-1) - 369 t:959
  QV(-1) - 12207 t:1190
  QT(-1) - 403 t:1080
  QC(-1) - 0 t:970
  QAS(-1) - 0 t:880
  QAH(-1) - 4 t:949
  QAA(-1) - 100 t:1273
  QAD(-1) - 100 t:1019
  QEM(-1) - 1 t:924
  QF(1) - 0 t:860
  QF(2) - 0 t:861
  QF(3) - 369.0.0 t:1268
Total Time: 29175

Cmd: 8 1

Servo 1 values
Q(-1) - 1
QO(-1) - 0
QAR(-1) - 1800
QP(-1) - 1468
QD(-1) - -29
QWD(-1) - 0
QWR(-1) - 1
QS(-1) - 7
QSD(-1) - 600
QSR(-1) - 100
QLED(-1) - 0
QG(-1) - 1
QID(-1) - 1
QB(-1) - 250000
QFD(-1) - DIS
QMS(-1) - LSS-HS1
QF(-1) - 369
QV(-1) - 12209
QT(-1) - 403
QC(-1) - 0
QAS(-1) - 0
QAH(-1) - 4
QAA(-1) - 100
QAD(-1) - 100
QEM(-1) - 1
QF(1) - 0
QF(2) - 0
QF(3) - 369.0.0
Total Time: 20254

Cmd: 9 2

Servo 2 values
  Q(-1) - 1 t:759
  QO(-1) - 0 t:784
  QAR(-1) - 1800 t:1096
  QP(-1) - 1485 t:973
  QD(-1) - -14 t:927
  QWD(-1) - 0 t:877
  QWR(-1) - 0 t:958
  QS(-1) - 0 t:797
  QSD(-1) - -600 t:1096
  QSR(-1) - -100 t:1084
  QLED(-1) - 0 t:1039
  QG(-1) - -1 t:909
  QID(-1) - 2 t:963
  QB(-1) - -12144 t:1493
  QFD(-1) - DIS t:1717
  QMS(-1) - LSS-HS1 t:1152
  QF(-1) - 369 t:959
  QV(-1) - 12287 t:1191
  QT(-1) - 430 t:1092
  QC(-1) - 0 t:1003
  QAS(-1) - 0 t:900
  QAH(-1) - 4 t:963
  QAA(-1) - 100 t:1019
  QAD(-1) - 100 t:1019
  QEM(-1) - 1 t:949
  QF(1) - 0 t:861
  QF(2) - 0 t:857
  QF(3) - 369.0.0 t:1193
Total Time: 28752

Cmd: 8 2

Servo 2 values
Q(-1) - 1
QO(-1) - 0
QAR(-1) - 1800
QP(-1) - 1485
QD(-1) - -14
QWD(-1) - 0
QWR(-1) - 0
QS(-1) - 3
QSD(-1) - -600
QSR(-1) - -100
QLED(-1) - 0
QG(-1) - -1
QID(-1) - 2
QB(-1) - 250000
QFD(-1) - DIS
QMS(-1) - LSS-HS1
QF(-1) - 369
QV(-1) - 12288
QT(-1) - 430
QC(-1) - 0
QAS(-1) - 0
QAH(-1) - 4
QAA(-1) - 100
QAD(-1) - 100
QEM(-1) - 1
QF(1) - 0
QF(2) - 0
QF(3) - 369.0.0
Total Time: 20335

Will now for fun of change baud rate, maybe start back at 500kb

[kurte]

@kurte

Ran it after servos updated: on servos 1 and 2.
9 1 Total Time: 29175
8 1 Total Time: 20254
9 2 Total Time: 28752
9 2 Total Time: 20335

Again above was at 250KB

Updated servos to 500kb using my change baud sketch…
New timings:

Cmd: 9 1
Total Time: 22920
Cmd: 8 1
Total Time: 20220

Cmd: 9 2
Total Time: 22742
Cmd: 8 2
Total Time: 19962

So for sure helped with individual query calls, not as much difference with the large initial string…

Edit: Added 750kb

Cmd: 9 1

Total Time: 21096:
Cmd: 8 1

Total Time: 19876
Cmd: 9 2
Total Time: 21155
Cmd: 8 2
Total Time: 20386

A little faster…

Will leave at this for now

[dialfonzo]

@kurte - Thanks for the test. Some queries are quite slow to respond and that hasn’t been touched in this update however maybe our team can do something about it, I’ll keep you posted when I have more info.

Don’t be shy, crank it up to 921600 and see.
Our test code, on 368, started to have failure even at 230400 most of the time QN and getting worst after that.

However… No issues on 369 at 921600

[kurte]

Thanks will bump it up soon.

One thing that I am not sure how to test, is knowing if the commands I sent are received by the servos… And how fast can I send #1D50\r like commands and have the servo react to them… That is if I send 100 positions to the servos will it process all 100 of them assuming that the packets are received?

[dialfonzo]

@kurte

That is if I send 100 positions to the servos will it process all 100 of them assuming that the packets are received?

I’m pretty sure it will, unless you continue to spam it at the same rate.

@scharette - Any idea about this?

[kurte]

@madmax

@madmax

I remember you mentioning the ROS one before but I never looked into it. I will have a look and see what is done there.

I should probably mention again that code base was for ROS. Not sure if Kevin has tried to run his stuff on Ros2.

Another complete code base I keep meaning to try out, is the newer ones for ROS, that Trossen has released for their higher end Hexapods, such as: PhantomX Hexapod MK-IV
Although I am not sure yet, but it also may be for ROS and not ROS2…
That is they mention it running on Ubuntu 20.04 with ROS Noetic…

Will be interesting to see if they have started migrating to ROS2 or not… But in the mean time they look like they have quite a few packages and the like to look at.

@cyberpalin - Sorry it took me a few days to get to post office. But I did get there today. I believe the other parts shipped several days ago and I believe should arrive any day now… Should be fun!

[cyberpalin]

@kurte

@cyberpalin - Sorry it took me a few days to get to post office. But I did get there today. I believe the other parts shipped several days ago and I believe should arrive any day now… Should be fun!

@kurte - just to let you know got the care package with all the parts, less servos, in the post today. Thank you. This should be fun to play with a working crawler.

[kurte]

@dialfonzo @cyberpalin and all:

@dialfonzo

Don’t be shy, crank it up to 921600 and see.
Our test code, on 368, started to have failure even at 230400 most of the time QN and getting worst after that.

However… No issues on 369 at 921600

I bumped it up to 921600 and did not try the simple register speed test… Assumption probably a bit faster…

However I tried out the:
simple moves:
First the command c - to configure
then i to run through about 5 positions… During this we repeat doing a query to wait until we get to state 6…

Cmd: c
Cmd: i
<421, 163 75> <18000, 10286 9000> <421, 4000 316> <1385, 621 160> <14400, 24000 24000> <1385, 139 442> 
get status for Coxa servo:2 for leg:3 failed on loop: 6
get status for Tibia servo:11 for leg:2 failed on loop: 9
get status for Femur servo:10 for leg:5 failed on loop: 48
get status for Femur servo:10 for leg:5 failed on loop: 55
get status for Tibia servo:5 for leg:0 failed on loop: 97
get status for Tibia servo:5 for leg:0 failed on loop: 98
get status for Tibia servo:5 for leg:0 failed on loop: 102
get status for Tibia servo:5 for leg:0 failed on loop: 109
get status for Tibia servo:5 for leg:0 failed on loop: 120
get status for Tibia servo:5 for leg:0 failed on loop: 124
get status for Tibia servo:5 for leg:0 failed on loop: 193
get status for Tibia servo:5 for leg:0 failed on loop: 206
Checks Status wait loops 207 in us: 1466926

Print Servo Positions Joint(Goal, timed, end)
C:1(0, 7, 8)	F:3(-900, -892, -892)	T:5(-600, 29, -594)
C:13(0, -4, -4)	F:15(-900, -893, -893)	T:17(-600, -592, -592)
C:1(0, 7, 7)	F:9(-900, -904, -904)	T:11(-600, -592, -592)
C:2(0, 3, 3)	F:4(-900, -895, -895)	T:6(-600, -593, -593)
C:14(0, -5, -5)	F:16(-900, -897, -896)	T:18(-600, -597, -597)
C:2(0, 3, 3)	F:10(-900, -888, -893)	T:12(-600, -598, -599)
<720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> 
get status for Coxa servo:1 for leg:0 failed on loop: 2
get status for Coxa servo:14 for leg:4 failed on loop: 3
get status for Tibia servo:17 for leg:1 failed on loop: 8
get status for Femur servo:3 for leg:0 failed on loop: 10
Checks Status wait loops 11 in us: 476313

Print Servo Positions Joint(Goal, timed, end)
C:1(100, 0, 93)	F:3(-450, -900, -470)	T:5(-300, -600, -316)
C:13(100, 0, 110)	F:15(-450, -900, -488)	T:17(-300, -600, -325)
C:1(100, 0, 93)	F:9(-450, -900, -466)	T:11(-300, -600, -323)
C:2(100, 0, 97)	F:4(-450, -900, -895)	T:6(-300, -600, -320)
C:14(100, 0, 109)	F:16(-450, -900, -464)	T:18(-300, -600, -309)
C:2(100, 0, 96)	F:10(-450, -900, -476)	T:12(-300, -600, -306)
<720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> 
get status for Femur servo:16 for leg:4 failed on loop: 2
get status for Tibia servo:6 for leg:3 failed on loop: 5
get status for Femur servo:15 for leg:1 failed on loop: 6
get status for Femur servo:3 for leg:0 failed on loop: 15
get status for Femur servo:4 for leg:3 failed on loop: 43
get status for Femur servo:4 for leg:3 failed on loop: 118
Checks Status wait loops 119 in us: 770892

Print Servo Positions Joint(Goal, timed, end)
C:1(200, 206, 206)	F:3(0, 7, 9)	T:5(0, 6, 6)
C:13(200, 205, 203)	F:15(0, -2, 1)	T:17(0, 5, 0)
C:1(200, 206, 206)	F:9(0, 0, 2)	T:11(0, 6, 8)
C:2(200, 199, 199)	F:4(0, -359, -6)	T:6(0, 8, 8)
C:14(200, 202, 202)	F:16(0, 4, 4)	T:18(0, 3, 3)
C:2(200, 199, 199)	F:10(0, 3, 4)	T:12(0, 3, 3)
<360, 160 160> <360, 160 160> <360, 160 160> <360, 160 160> <360, 160 160> <360, 160 160> 
get status for Coxa servo:14 for leg:4 failed on loop: 2
get status for Coxa servo:1 for leg:0 failed on loop: 3
get status for Femur servo:16 for leg:4 failed on loop: 4
get status for Femur servo:4 for leg:3 failed on loop: 12
Checks Status wait loops 13 in us: 519228

Print Servo Positions Joint(Goal, timed, end)
C:1(0, -17, -17)	F:3(450, 432, 434)	T:5(450, 446, 448)
C:13(0, -14, -13)	F:15(450, 444, 448)	T:17(450, 447, 449)
C:1(0, -17, -17)	F:9(450, 445, 447)	T:11(450, 444, 447)
C:2(0, -8, -8)	F:4(450, 425, 427)	T:6(450, 448, 449)
C:14(0, -8, -8)	F:16(450, 0, 442)	T:18(450, 451, 451)
C:2(0, -8, -8)	F:10(450, 445, 445)	T:12(450, 448, 448)
<720, 160 160> <720, 160 160> <720, 160 160> <720, 160 160> <720, 160 160> <720, 160 160> 
get status for Coxa servo:13 for leg:1 failed on loop: 1
get status for Coxa servo:14 for leg:4 failed on loop: 1
get status for Tibia servo:6 for leg:3 failed on loop: 2
get status for Tibia servo:18 for leg:4 failed on loop: 2
get status for Tibia servo:6 for leg:3 failed on loop: 3
Checks Status wait loops 4 in us: 642861

Print Servo Positions Joint(Goal, timed, end)
C:1(-100, -96, -95)	F:3(0, 32, 31)	T:5(0, 15, 10)
C:13(-100, 0, -96)	F:15(0, 17, 17)	T:17(0, 15, 15)
C:1(-100, -95, -96)	F:9(0, 22, 22)	T:11(0, 15, 16)
C:2(-100, -86, -86)	F:4(0, 62, 62)	T:6(0, 8, 8)
C:14(-100, -101, -101)	F:16(0, 27, 27)	T:18(0, 10, 10)
C:2(-100, -86, -86)	F:10(0, 15, 15)	T:12(0, 10, 10)
<720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> 
get status for Tibia servo:12 for leg:5 failed on loop: 3
get status for Tibia servo:12 for leg:5 failed on loop: 4
get status for Femur servo:10 for leg:5 failed on loop: 6
get status for Femur servo:10 for leg:5 failed on loop: 11
Checks Status wait loops 12 in us: 477762

Print Servo Positions Joint(Goal, timed, end)
C:1(-200, -100, -202)	F:3(-450, 0, -458)	T:5(-300, 0, -302)
C:13(-200, -100, -204)	F:15(-450, 0, -446)	T:17(-300, 0, -37)
C:1(-200, -100, -202)	F:9(-450, 0, 22)	T:11(-300, 0, -305)
C:2(-200, -100, -215)	F:4(-450, 0, -434)	T:6(-300, 0, -301)
C:14(-200, -100, -205)	F:16(-450, 0, -459)	T:18(-300, 0, -305)
C:2(-200, -100, -215)	F:10(-450, 0, -457)	T:12(-300, 0, -301)
<360, 160 240> <360, 160 240> <360, 160 240> <360, 160 240> <360, 160 240> <360, 160 240> 
get status for Femur servo:4 for leg:3 failed on loop: 1
get status for Tibia servo:6 for leg:3 failed on loop: 1
get status for Tibia servo:18 for leg:4 failed on loop: 1
get status for Femur servo:16 for leg:4 failed on loop: 2
get status for Tibia servo:17 for leg:1 failed on loop: 22
get status for Femur servo:9 for leg:2 failed on loop: 89
get status for Femur servo:9 for leg:2 failed on loop: 104
Checks Status wait loops 105 in us: 822442

Print Servo Positions Joint(Goal, timed, end)
C:1(0, 16, 16)	F:3(-900, -893, -893)	T:5(-600, -595, -595)
C:13(0, 8, 8)	F:15(-900, -902, -902)	T:17(-600, -598, -598)
C:1(0, 16, 16)	F:9(-900, -631, -907)	T:11(-600, -600, -600)
C:2(0, 18, 17)	F:4(-900, -899, -899)	T:6(-600, -600, -600)
C:14(0, 14, 13)	F:16(-900, -902, -902)	T:18(-600, -598, -598)
C:2(0, 18, 18)	F:10(-900, -899, -899)	T:12(-600, -599, -599)

As you can see the Q command failed several times… I will look tomorrow with Logic Analyzer. Note: I tried it a couple of time and at times some servos went in slow motion the opposite direction

I then tried the K command, which did not do the Q’s but instead output many positions with EM=0… Don’t do Q so not that failure, but did see some servos appear to go smoothly. Maybe again a command was not received correctly, but could not go far as output like 50 frames per second.

All for tonight

[kurte]

@dialfonzo @cbenson @cyberpalin and all follow on to last nights run at the higher speed…

@kurte

I bumped it up to 921600 and did not try the simple register speed test… Assumption probably a bit faster…

However I tried out the:
simple moves:
First the command c - to configure
then i to run through about 5 positions… During this we repeat doing a query to wait until we get to state 6

I ran the Servo test program again at the high speed.
Again ran the ‘c’ command to configure servos
and then the ‘i’ command to step the servos through a few positions. Looks like I have it configured right now to try to use move speed instead of time…
That is for each of the servos: we have tried a few different ways:

          myLSS.setServoID(legs[leg].coxa.id);
          // compute speed... 
          int delta_pos = abs(legs[leg].coxa.time_position - rf_stance[position][0]); 
          int move_speed = ComputeServoMoveSpeed(delta_pos, servo_move_time);
          LSS::genericWrite(myLSS.getServoID(), LSS_ActionMove, rf_stance[position][0], "SD", move_speed);
          //myLSS.moveT(rf_stance[position][0], servo_move_time);
          //myLSS.move(rf_stance[position][0]);

Again stances are nothing special…

  {0, -900, -600},       //Low
  // 0, -900, 510 degrees
  {100, -450, -300},      //mid Low
  {200,  0,  0},          //Med
  {0,  450, 450},         //High
  { -100,  0,  0},        //Med
  { -200, -450, -300},    //mid Low
  {0, -900, -600}       //Low

For each leg 3 numbers: coxa femur tibia.

And once in this pretzel state, several of the servos cut out, and was in endless query…
Will probably next try reducing back to 500kb.

But you might try it there are your setup to see if you see similar results.

Not sure how much more I will get done today

[kurte]

One last update for this morning…

OK I updated the servos to go back to 500kb… Motions better. But I am still getting failures on some of the reads.

Cmd: i
<4235, 72000 72000> <8000, 72000 3600> <4500, 3600 36000> <7200, 36000 3600> <7200, 3600 3600> <7200, 36000 72000> 
get status for Coxa servo:1 for leg:2 failed on loop: 42
Checks Status wait loops 43 in us: 147314

Print Servo Positions Joint(Goal, timed, end)
C:1(0, 17, 1)	F:3(-900, -901, -901)	T:5(-600, -599, -599)
C:13(0, 9, 10)	F:15(-900, -901, -901)	T:17(-600, -600, -600)
C:1(0, 16, 1)	F:9(-900, -900, -900)	T:11(-600, -598, -598)
C:2(0, 10, 8)	F:4(-900, -898, -898)	T:6(-600, -600, -600)
C:14(0, 10, 6)	F:16(-900, -900, -900)	T:18(-600, -600, -601)
C:2(0, 10, 8)	F:10(-900, -902, -902)	T:12(-600, -599, -599)
<720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> 
get status for Tibia servo:6 for leg:3 failed on loop: 31
Checks Status wait loops 61 in us: 446505

Print Servo Positions Joint(Goal, timed, end)
C:1(100, 0, 109)	F:3(-450, -900, -517)	T:5(-300, -600, -315)
C:13(100, 0, 101)	F:15(-450, -900, -501)	T:17(-300, -600, -321)
C:1(100, 0, 108)	F:9(-450, -900, -515)	T:11(-300, -600, -319)
C:2(100, 0, 94)	F:4(-450, -900, -499)	T:6(-300, -600, -315)
C:14(100, 0, 102)	F:16(-450, -900, -484)	T:18(-300, -600, -313)
C:2(100, 0, 94)	F:10(-450, -900, -493)	T:12(-300, -600, -309)
<720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> 
Checks Status wait loops 63 in us: 448864

Print Servo Positions Joint(Goal, timed, end)
C:1(200, 100, 205)	F:3(0, -450, -23)	T:5(0, -300, 7)
C:13(200, 100, 205)	F:15(0, -450, -25)	T:17(0, -300, 5)
C:1(200, 100, 205)	F:9(0, -450, -16)	T:11(0, -300, 7)
C:2(200, 100, 206)	F:4(0, -450, -18)	T:6(0, -300, 8)
C:14(200, 100, 207)	F:16(0, -450, -14)	T:18(0, -300, 7)
C:2(200, 100, 206)	F:10(0, -450, -24)	T:12(0, -300, 3)
<360, 160 160> <360, 160 160> <360, 160 160> <360, 160 160> <360, 160 160> <360, 160 160> 
get status for Coxa servo:1 for leg:0 failed on loop: 1
get status for Coxa servo:1 for leg:2 failed on loop: 9
get status for Coxa servo:1 for leg:0 failed on loop: 10
Checks Status wait loops 42 in us: 447062

Print Servo Positions Joint(Goal, timed, end)
C:1(0, 200, -17)	F:3(450, 0, 412)	T:5(450, 0, 416)
C:13(0, 200, -11)	F:15(450, 0, 417)	T:17(450, 0, 415)
C:1(0, 200, -17)	F:9(450, 0, 411)	T:11(450, 0, 422)
C:2(0, 200, -9)	F:4(450, 0, 404)	T:6(450, 0, 422)
C:14(0, 200, -17)	F:16(450, 0, 423)	T:18(450, 0, 421)
C:2(0, 200, -9)	F:10(450, 0, 421)	T:12(450, 0, 425)
<720, 160 160> <720, 160 160> <720, 160 160> <720, 160 160> <720, 160 160> <720, 160 160> 
get status for Coxa servo:2 for leg:3 failed on loop: 4
get status for Coxa servo:2 for leg:5 failed on loop: 6
Checks Status wait loops 45 in us: 448668

Print Servo Positions Joint(Goal, timed, end)
C:1(-100, 0, -97)	F:3(0, 450, 61)	T:5(0, 450, 55)
C:13(-100, 0, -97)	F:15(0, 450, 49)	T:17(0, 450, 53)
C:1(-100, 0, -97)	F:9(0, 450, 53)	T:11(0, 450, 52)
C:2(-100, 0, -96)	F:4(0, 450, 73)	T:6(0, 450, 44)
C:14(-100, 0, -98)	F:16(0, 450, 42)	T:18(0, 450, 54)
C:2(-100, 0, -96)	F:10(0, 450, 42)	T:12(0, 450, 47)
<720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> <720, 160 240> 
get status for Coxa servo:1 for leg:2 failed on loop: 17
Checks Status wait loops 74 in us: 462002

Print Servo Positions Joint(Goal, timed, end)
C:1(-200, -100, -201)	F:3(-450, 0, -427)	T:5(-300, 0, -301)
C:13(-200, -100, -204)	F:15(-450, 0, -442)	T:17(-300, 0, -301)
C:1(-200, -100, -201)	F:9(-450, 0, -439)	T:11(-300, 0, -301)
C:2(-200, -100, -203)	F:4(-450, 0, -431)	T:6(-300, 0, -300)
C:14(-200, -100, -197)	F:16(-450, 0, -457)	T:18(-300, 0, -305)
C:2(-200, -100, -203)	F:10(-450, 0, -438)	T:12(-300, 0, -302)
<360, 160 240> <360, 160 240> <360, 160 240> <360, 160 240> <360, 160 240> <360, 160 240> 
get status for Femur servo:3 for leg:0 failed on loop: 55
Checks Status wait loops 56 in us: 443665

Print Servo Positions Joint(Goal, timed, end)
C:1(0, -200, 18)	F:3(-900, -450, -865)	T:5(-600, -300, -597)
C:13(0, -200, 5)	F:15(-900, -450, -874)	T:17(-600, -300, -600)
C:1(0, -200, 17)	F:9(-900, -450, -873)	T:11(-600, -300, -601)
C:2(0, -200, 13)	F:4(-900, -450, -882)	T:6(-600, -300, -600)
C:14(0, -200, 8)	F:16(-900, -450, -881)	T:18(-600, -300, -599)
C:2(0, -200, 13)	F:10(-900, -450, -891)	T:12(-600, -30

Each transition on the red(3) line is when a query failed:

But in most of the cases it appears like the query is not failing doing to corruption, but because the servo did not respond.
…
Which you can see in the analyzer.

[cyberpalin]

@dialfonzo @cbenson @kurte and all

Was a bit distracted yesterday but this morning I went back to the LSS_M0_TSTEP sketch to test getPosition with kind of sort of timed moves which is based on the sketch @dialfonzo provided. The updated Sketch is in @kurte repository.

Any every 25ms the servo is moving to a specific position and then the sketch does a getPostion to verify the move but the commanded move position isn’t matching the position returned by the getPosition or the queryLSS function. Here are the results:

Sequence Start

FRAME 1
--------------
Returned Postition: 446
goal: -450, time: 250, pos_num: 10
Current pos: (qd: 446), dPos(-896), tStep(-89)
i: 1, move: 357
Returned Postition: 432
i: 2, move: 268
Returned Postition: 386
i: 3, move: 179
Returned Postition: 323
i: 4, move: 90
Returned Postition: 248
i: 5, move: 1
Returned Postition: 169
i: 6, move: -88
Returned Postition: 84
i: 7, move: -177
Returned Postition: -1
i: 8, move: -266
Returned Postition: -85
i: 9, move: -355
Returned Postition: -171
i: 10, move: -444
Returned Postition: -256
Final Move: -450
Time(ms)/ Pos(deg)
259 / 446, 432, 386, 323, 248, 169, 84, -1, -85, -171, -266

FRAME 2
--------------
Returned Postition: -447
goal: 450, time: 250, pos_num: 10
Current pos: (qd: -447), dPos(897), tStep(89)
i: 1, move: -358
Returned Postition: -429
i: 2, move: -269
Returned Postition: -382
i: 3, move: -180
Returned Postition: -321
i: 4, move: -91
Returned Postition: -250
i: 5, move: -2
Returned Postition: -173
i: 6, move: 87
Returned Postition: -94
i: 7, move: 176
Returned Postition: -8
i: 8, move: 265
Returned Postition: 76
i: 9, move: 354
Returned Postition: 167
i: 10, move: 443
Returned Postition: 257
Final Move: 450
Time(ms)/ Pos(deg)
259 / -447, -429, -382, -321, -250, -173, -94, -8, 76, 167, 265

Frame one sweeps one direction and frame 2 sweeps in reverse. In both cases the start position seems to be correct but the intermediate positions seem to be off.

If we look at the LA trace of the commands it confirms this:

• 24.9ms between move to 357 and request for position
• 496.24 us from request for position to response of 432
• 86 us for response
• 8.4 us to next move after response

No errors were seen in the responses. But you will notice that the initial start response is correct of 44.6 degrees (close to the 450 assumed).

EDIT: Forgot I am running at 921K

[cyberpalin]

@cyberpalin

@dialfonzo @cbenson @kurte and all

@dialfonzo @cbenson @kurte and all
Just as a quick follow-up to my previous post I added in a checkStatus function the will loop until the hold position is reached for the Servo - in all cases it was at which point getPosition is called. So not sure what the reason is.

Initialized. Firmware Version: v1.0
Initial Reset: 
Set AngularStiffness to -4: 

Set Holding Stiffness to 4: 
Set Motion Control: 
Do not Set FPC: 
Sequence Start

FRAME 1
--------------
Returned Postition: 446
goal: -450, time: 250, pos_num: 10
Current pos: (qd: 446), dPos(-896), tStep(-89)
i: 1, move: 357
Status Loop CNT: 1: 6
Returned Postition: 438
i: 2, move: 268
Status Loop CNT: 1: 6
Returned Postition: 408
i: 3, move: 179
Status Loop CNT: 1: 6
Returned Postition: 365
i: 4, move: 90
Status Loop CNT: 1: 6
Returned Postition: 317
i: 5, move: 1
Status Loop CNT: 1: 6
Returned Postition: 260
i: 6, move: -88
Status Loop CNT: 1: 6
Returned Postition: 196
i: 7, move: -177
Status Loop CNT: 1: 6
Returned Postition: 127
i: 8, move: -266
Status Loop CNT: 1: 6
Returned Postition: 52
i: 9, move: -355
Status Loop CNT: 1: 6
Returned Postition: -27
i: 10, move: -444
Status Loop CNT: 1: 6
Returned Postition: -109
Final Move: -450
Time(ms)/ Pos(deg)
264 / 446, 438, 408, 365, 317, 260, 196, 127, 52, -27, -119

FRAME 2
--------------
Returned Postition: -447
goal: 450, time: 250, pos_num: 10
Current pos: (qd: -447), dPos(897), tStep(89)
i: 1, move: -358
Status Loop CNT: 1: 6
Returned Postition: -438
i: 2, move: -269
Status Loop CNT: 1: 6
Returned Postition: -411
i: 3, move: -180
Status Loop CNT: 1: 6
Returned Postition: -371
i: 4, move: -91
Status Loop CNT: 1: 6
Returned Postition: -323
i: 5, move: -2
Status Loop CNT: 1: 6
Returned Postition: -265
i: 6, move: 87
Status Loop CNT: 1: 6
Returned Postition: -202
i: 7, move: 176
Status Loop CNT: 1: 6
Returned Postition: -135
i: 8, move: 265
Status Loop CNT: 1: 6
Returned Postition: -62
i: 9, move: 354
Status Loop CNT: 1: 6
Returned Postition: 18
i: 10, move: 443
Status Loop CNT: 1: 6
Returned Postition: 102
Final Move: 450
Time(ms)/ Pos(deg)
265 / -447, -438, -411, -371, -323, -265, -202, -135, -62, 18, 111

unless there is something wrong with my assumptions - I am pushing that change up as well with the checkStatus function I am using.

I am not seeing the same issues as @kurte more than likely because I am only using 3 servos, might be a comm issue when testing with 18 servos.

[kurte]

@cyberpalin

I am not seeing the same issues as @kurte more than likely because I am only using 3 servos, might be a comm issue when testing with 18 servos.

Yep could be something like that:
Likewise wonder if there could be issues with timing differences with the servos?
That is in the last run I had 9 Q commands fail to return data:

5 of them were on Servo 1 (Coxa HS)
2 of them was on Servo 2 (Coxa HS)
1 on #3 Femur (ST)
1 on #6 Tiba (ST)

Note: the coxas are the first ones connected up to the board, so you would think it would have the best signal…

More testing tomorrow…

[madmax]

hi @kurte @cyberpalin

I was trying to understand the usbjoystick code and I wanted to knows what is required to get the USBhost working.

I did a separate sketch and don’t seem to get it working.

For a minimum I have

#include <USBHost_t36.h>

// lets define Buttons and Axis mapping for different joysticks.
enum {
  BUT_L1 = 0, 
  BUT_L2,
  BUT_L3,
  BUT_R1,
  BUT_R2,
  BUT_R3,
  BUT_TRI,
  BUT_SQ,
  BUT_X,
  BUT_CIRC,
  BUT_PS3,
  BUT_SELECT,
  BUT_START,
  BUT_HAT_UP,
  BUT_HAT_DOWN,
  BUT_HAT_LEFT,
  BUT_HAT_RIGHT
};
enum { AXIS_LX, AXIS_LY, AXIS_RX, AXIS_LT, AXIS_RT, AXIS_RY };  // Order of PS3

const static uint32_t PS3_BTNS[] = { 
  0x400, 
  0x100,
  0x2,
  0x800,
  0x200,
  0x4,
  0x1000,
  0x8000,
  0x4000,
  0x2000,
  0x10000,
  0x1,
  0x8,
  // UP  DN    LFT   RHT
  0x10, 0x40, 0x80, 0x20
};

const static uint32_t PS4_BTNS[] = {
  0x10,
  0x40,
  0x400,
  0x20,
  0x80,
  0x800,    //L1,L2,??? , R1, R2 ?????
  0x8,
  0x1,
  0x2,
  0x4,//tri, square, cross, circle
  0x1000,
  0x200,
  0x100,       // PS, options, Share
  0x10000,
  0x40000,
  0x80000,
  0x20000       // HAT - up, down, left, right
};
const static uint32_t PS4_MAP_HAT_MAP[] = {
  //0x10, 0x30, 0x20, 0x60, 0x40, 0xc0, 0x80, 0x90, 0x00 };
  0x10000, 0x30000, 0x20000, 0x60000, 0x40000, 0xC0000, 0x80000, 0x90000, 0x0 };
  //up,    NE,      right,   SE,      down,    SW,      left,    NW,   ???
 uint32_t const * BTN_MASKS = PS3_BTNS;
 static bool joystick_ps4_bt = true;

//=============================================================================
// Global - Local to this file only...
//=============================================================================
USBHost myusb;
USBHub hub1(myusb);
USBHub hub2(myusb);
USBHIDParser hid1(myusb);
USBHIDParser hid2(myusb);
USBHIDParser hid3(myusb);
JoystickController joystick1(myusb);
//BluetoothController bluet(myusb);   // version assumes it already was paired

USBDriver* drivers[] = { &hub1, &hub2, &joystick1, &hid1, &hid2, &hid3 };
  
#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
const char* driver_names[CNT_DEVICES] = { "Hub1", "Hub2", "JOY1D", "HID1" , "HID2", "HID3" };
bool driver_active[CNT_DEVICES] = { false, false, false };

int user_axis[64];
uint32_t g_buttons_prev = 0;
uint32_t g_buttons;
bool first_joystick_message = true;
uint8_t last_bdaddr[6] = { 0, 0, 0, 0, 0, 0 };

// Lets also look at HID Input devices
USBHIDInput* hiddrivers[] = { &joystick1 };

#define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))
const char* hid_driver_names[CNT_DEVICES] = { "Joystick1" };
bool hid_driver_active[CNT_DEVICES] = { false };

unsigned long g_ulLastMsgTime;
short  g_sGPSMController;    // What GPSM value have we calculated. 0xff - Not used yet
boolean g_fDynamicLegXZLength = false;  // Has the user dynamically adjusted the Leg XZ init pos (width)
boolean g_fDebugJoystick = false;

static short   g_BodyYOffset;
static short   g_BodyYShift;
static byte    ControlMode;
static byte    HeightSpeedMode;
//static bool  DoubleHeightOn;
static bool    DoubleTravelOn;
static byte    bJoystickWalkMode;
byte           GPSeq;             //Number of the sequence


void UpdateActiveDeviceInfo()
{
  for (uint8_t i = 0; i < CNT_DEVICES; i++)
  { 
    Serial.print("Inside the active device info..");
    Serial.println(i);
    Serial.print("*drivers[i]= ");
    Serial.println(*drivers[i]);
    Serial.print("driver_active[i] = ");
    Serial.println(driver_active[i]);
    
    if (*drivers[i] != driver_active[i])
    {
      Serial.println("Inside the active device info..1");
      if (driver_active[i])
      {
        Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
        driver_active[i] = false;
      }
      else
      {
        Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
        driver_active[i] = true;

        const uint8_t *psz = drivers[i]->manufacturer();
        if (psz && *psz)
          DBGSerial.printf("  manufacturer: %s\n", psz);
        psz = drivers[i]->product();
        if (psz && *psz)
          DBGSerial.printf("  product: %s\n", psz);
        psz = drivers[i]->serialNumber();
        if (psz && *psz)
          DBGSerial.printf("  Serial: %s\n", psz);
      }
    }
  }

  for (uint8_t i = 0; i < CNT_HIDDEVICES; i++)
  {
    if (*hiddrivers[i] != hid_driver_active[i])
    {
      if (hid_driver_active[i])
      {
        DBGSerial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
        hid_driver_active[i] = false;
      }
      else
      {
        DBGSerial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
        hid_driver_active[i] = true;

        const uint8_t *psz = hiddrivers[i]->manufacturer();
        if (psz && *psz)
          DBGSerial.printf("  manufacturer: %s\n", psz);
        psz = hiddrivers[i]->product();
        if (psz && *psz)
          DBGSerial.printf("  product: %s\n", psz);
        psz = hiddrivers[i]->serialNumber();
        if (psz && *psz)
          DBGSerial.printf("  Serial: %s\n", psz);

        // See if this is our joystick object...
        if (hiddrivers[i] == &joystick1)
        {
          DBGSerial.printf("  Joystick type: %d\n", joystick1.joystickType());
          switch (joystick1.joystickType())
          {
          case JoystickController::PS4:
            BTN_MASKS = PS4_BTNS;
            break;
          default:
          case JoystickController::PS3:
            BTN_MASKS = PS3_BTNS;
            break;
          }

#ifdef LATER
          if (joystick1.joystickType() == JoystickController::PS3_MOTION)
          {
            DBGSerial.println("  PS3 Motion detected");
            PS3_MOTION_timer = millis(); // set time for last event
            PS3_MOTION_tried_to_pair_state = 0;
          }
#endif
        }
      }
    }
  }
}


// If both PS2 and XBee are defined then we will become secondary to the xbee
void InitUSBJoystick(void)
{
#ifdef DBGSerial  
  DBGSerial.print("USB Joystick Init: ");
#endif
  myusb.begin();
}

void USBJoystickControlInput(void)
{
  DBGSerial.println("USBJOystickControlINput Enter:.... ");
  // Make sure USB gets chance to process stuff. 
  myusb.Task();
  // check to see if the device list has changed:
  DBGSerial.println(" BEfore update device info ");
  UpdateActiveDeviceInfo();
  DBGSerial.println("After update device info ");
  //  processPS3MotionTimer();  - not sure yet support this yet. 
  Serial.printf("*** Joystick Avaialbele %d", joystick1.available());
  if (!joystick1.available())
  {
    if (first_joystick_message) 
    {
      Serial.printf("*** First Joystick message %x:%x ***\n", joystick1.idVendor(), joystick1.idProduct());
      first_joystick_message = false;

      const uint8_t* psz = joystick1.manufacturer();
      if (psz && *psz)
      {
        Serial.printf("  manufacturer: %s\n", psz);
      }
      const uint8_t* pszProduct = joystick1.product();
      if (pszProduct && *pszProduct)
      {
        Serial.printf("  product: %s\n", pszProduct);
      }
      psz = joystick1.serialNumber();
      if (psz && *psz)
      {
        Serial.printf("  Serial: %s\n", psz);
      }
      if (pszProduct && strncmp((const char *)pszProduct, "Wireless Controller", 19) == 0)
      {
        BTN_MASKS = PS4_BTNS;
        // hack alert - but if we don't have BT object as active it won't be a BT PS4       
        if (0)
        {
          joystick_ps4_bt = true;
          DBGSerial.println("      BlueTooth PS4");         
        }
        else
        {
          DBGSerial.println("      Wired PS4");         
        }
      }

      // lets try to reduce number of fields that update
      joystick1.axisChangeNotifyMask(0xFFFFFl);
    }


    // If we receive a valid message than turn robot on...
    boolean fAdjustLegPositions = false;
    short sLegInitXZAdjust = 0;
    short sLegInitAngleAdjust = 0;
    
    // [SWITCH MODES]
    g_buttons = joystick1.getButtons();
    Serial.printf("*** Joystick buttons %d", g_buttons);

void setup()
{
 
  // initialize the usb joystick
  InitUSBJoystick();
}

void loop()
{
  // put your main code here, to run repeatedly:
  // loop code
  USBJoystickControlInput();
}

This dosen’t seem to work as I get joystick1.avaialble() as false.
Can you please help ?
EDIT: I have tried and removed everything that is under Bluetooth define…

Thanks in advance!

[madmax]

@kurte @cyberpalin Thanks for your time. I think I managed to find it… Please ignore… It was just missing some delay after myusb.begin()… problem solved…

[cyberpalin]

@madmax
Just for future reference check out the examples in USBHost_t36 - there are several for wired and bt that make good reference. I use them as the base all the time. Sorry - just woke up a little while ago otherwise would have answered sooner. Glad you found your problem.

[kurte]

@madmax - Glad you got it working. Sorry I took the rest of the rest of yesterday off, with getting ready for some guests, then then guests, then cleaning up after…

As @cyberpalin mentioned there are several examples on this in the library. Some of the Bluetooth stuff will be evolving over time. So feel free to ask questions. Things like how to actually bind your PS4(or PS3) to the BT dongle…