Posted 24 December, 2022
As part of the suite of tools associated with wall tracking and IR beam homing, I created a set of ‘move to specified distance’ routines using my home-grown PID algorithm as follows:
- MoveToDesiredFrontDistCm (MTFD)
- MoveToDesiredRearDistCm(MTRD)
- MoveToDesiredLeftDistCm(MTLD)
- MoveToDesiredRightDistCm(MTRD)
I saw some odd problems in my past wall-tracking exercises, so I thought it would be a good idea to go back and test these in isolation to work out any bugs. As usual, I constructed a limited part-task program for this purpose, and ran a number of tests on my desktop ‘range’. I started with the ‘MoveToDesiredFrontDistCm()’ function, as shown below:
MoveToDesiredFrontDistCm ():
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 |
bool MoveToDesiredFrontDistCm(uint16_t offsetCm, float Kp, float Ki, float Kd) { //Purpose: Move forward or backward to a specified front offset distance // Inputs: // offsetCm - int denoting desired forward distance offset // Kp,Ki,Kd set at top of program (lines 85-87) // Outputs: // Robot moves forward or backward to the desired offset, under PID control //Notes: // 04/25/21 the Kp/Ki/Kd values are much lower than prescribed by Z-N theory // but I like the slower, more deliberate action. // 03/06/22 rev for Teensy 3.5 and PIDCalcs(). // 05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' positioning means // using a minus sign here for motor speed. float lastError = 0; float lastInput = 0; float lastIval = 0; float lastDerror = 0; //DEBUG!! gl_pSerPort->printf("MoveToDesiredFrontDistCm(%d): Kp/Ki/Kd= %2.2f/%2.2f/%2.2f\n", offsetCm, Kp, Ki, Kd); //DEBUG!! StopBothMotors(); //05/30/22 re-init front/back dist arrays to avoid spurious 'stuck' detection InitFrontDistArray(); InitRearDistArray(); gl_pSerPort->printf("MTFD: at start, tgt = %dcm, curr_dist = %d, front/rear var = %2.1f/%2.1f\n", offsetCm, GetFrontDistCm(), glFrontvar, glRearvar); gl_pSerPort->printf("Msec\tFdist\tTgtD\terr\tIval\tDerr\tOut\tSpeed\tFVar\tRVar\n"); UpdateAllEnvironmentParameters(); //AnomalyCode errcode = NO_ANOMALIES; AnomalyCode errcode = CheckForAnomalies(); MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate //06/12/22 bugfix: can't have GetFrontDistCm() in while() statement - cuz it gets executed too often //OffsetDistVal = (double)GetFrontDistCm(); //while (abs(GetFrontDistCm() - offsetCm) > 1 && errcode == NO_ANOMALIES) while (abs(glFrontCm - offsetCm) > 1 && errcode != STUCK_AHEAD && errcode != STUCK_BEHIND) { CheckForUserInput(); if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE) { MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE; UpdateAllEnvironmentParameters(); errcode = CheckForAnomalies(); OffsetDistVal = (double)GetFrontDistCm(); //gl_pSerPort->printf("MTFD: in loop, tgt = %dcm, curr_dist = %2.1f\n", offsetCm, OffsetDistVal); //OffsetDistOutput = PIDCalcs(OffsetDistVal, (float)offsetCm, lastError, // lastInput, lastIval, lastDerror, OffsetDistKp, OffsetDistKi, OffsetDistKd); OffsetDistOutput = PIDCalcs((double)glFrontCm, (float)offsetCm, lastError, lastInput, lastIval, lastDerror, Kp, Ki, Kd); int16_t speed = (int16_t)OffsetDistOutput; //gl_pSerPort->printf("MTFD: in loop, speed = %dcm\n", speed); //PIDCalcs out can be negative, must handle both signs if (speed >= 0) { speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed; speed = (speed <= MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : speed; } else //speed < 0 { speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed; } gl_pSerPort->printf("%lu\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\t%2.1f\t%2.1f\n", millis(), glFrontCm, offsetCm, lastError, lastIval, lastDerror, OffsetDistOutput, speed, glFrontvar, glRearvar); // 05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' requires the // use of a minus sign here for motor speed. RunBothMotorsBidirectional(-speed, -speed); } } //05/30/22 only worry about 'stuck' conditions //if (errcode == STUCK_AHEAD || errcode == STUCK_BEHIND) //{ // HandleAnomalousConditions(errcode, TRACKING_NONE); // return false; //} gl_pSerPort->printf("MTFD: Stopped with front dist = %d and errcode = %s\n", GetFrontDistCm(), AnomalyStrArray[errcode]); StopBothMotors(); return true; } |
Here’s some output from a typical run:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 |
Starting Front/BackSide Motion Test - enter Direction (1 = F, 2 = B, 3 = L, 4 = R), offset, Kp,Ki,Kd At top of parameter retrieval section DirStr = 1 OffStr = 60 Moving Front to 60 cm, with PID = (1.50,0.10,0.20) Enter any key to start Before MTDFD, FVar = 13062.84 MoveToDesiredFrontDistCm(60): Kp/Ki/Kd= 1.50/0.10/0.20 MTFD: at start, tgt = 60cm, curr_dist = 21, front/rear var = 13328.0/833.0 Msec Fdist TgtD err Ival Derr Out Speed FVar RVar 163417 20 60 40.0 4.0 40.0 56.0 56 13305.6 21444.6 163480 20 60 40.0 8.0 0.0 68.0 68 13974.0 30074.4 163530 20 60 40.0 12.0 0.0 72.0 72 14987.5 40126.6 163581 21 60 39.0 15.9 -1.0 74.6 74 16226.3 49659.8 163628 24 60 36.0 19.5 -3.0 74.1 74 18030.5 58677.5 163680 26 60 34.0 22.9 -2.0 74.3 74 19318.8 67183.6 163729 28 60 32.0 26.1 -2.0 74.5 74 20393.4 73500.8 163779 29 60 31.0 29.2 -1.0 75.9 75 21153.6 79398.7 163830 30 60 30.0 32.2 -1.0 77.4 75 21426.0 84880.5 163878 32 60 28.0 35.0 -2.0 77.4 75 20739.2 89949.6 163929 33 60 27.0 37.7 -1.0 78.4 75 19204.9 94609.4 163980 34 60 26.0 40.3 -1.0 79.5 75 16580.3 98863.2 164032 37 60 23.0 42.6 -3.0 77.7 75 12651.8 104153.8 164081 39 60 21.0 44.7 -2.0 76.6 75 7213.3 107565.6 164130 44 60 16.0 46.3 -5.0 71.3 71 53.6 110581.6 164181 49 60 11.0 47.4 -5.0 64.9 64 67.6 114521.3 164220 53 60 7.0 48.1 -4.0 59.4 59 86.6 117988.8 164281 53 60 7.0 48.8 0.0 59.3 59 99.7 120987.7 164333 57 60 3.0 49.1 -4.0 54.4 54 113.1 123521.6 164382 59 60 1.0 49.2 -2.0 51.1 51 124.7 124450.2 MTFD: Stopped with front dist = 59 and errcode = NO_ANOMALIES Front Rear Left Right 59 819.10 32.00 779.80 63 819.10 33.00 779.80 65 819.10 34.00 779.80 66 819.10 33.00 779.80 69 750.40 32.00 779.80 67 750.40 32.00 779.80 68 750.40 33.00 779.70 67 819.10 32.00 779.70 68 819.10 32.00 779.80 67 819.10 33.00 779.80 |
The movement goes OK, and the robot telemetry says it stopped very close to the desired 60cm distance. However, when I measured the actual distance from the ‘wall’ to the robot, I got more like 67 or 68cm, probably indicating that the robot coasted some after the motors were turned off. When I instrumented the code to show the next 10 distance measurements after the exit from the subroutine, it became easy to see that this was the case – the robot coasted from 59 to 67cm. The robot should correct this by going back the opposite way, but it doesn’t because the last measurement (59cm) fits inside the 59-61cm ‘basket’ for subroutine termination (the ‘while’ loop termination criteria).
So, I thought what I could do is check the reported front distance after function exit, and just call it again if the robot had coasted too far from the target. The second run should get much closer, as the starting error term would be much smaller. So now the test code looks like this:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
gl_pSerPort->printf("Before MTDFD, FVar = %2.2f\n", glFrontvar); MoveToDesiredFrontDistCm(Offset,Kp,Ki,Kd); //show 10 distances after stop gl_pSerPort->printf("Front\tRear\tLeft\tRight\n"); for (size_t i = 0; i < 10; i++) { GetRequestedVL53l0xValues(VL53L0X_ALL); glFrontCm = GetFrontDistCm(); gl_pSerPort->printf("%d\t%2.2f\t%2.2f\t%2.2f\n", glFrontCm, glRearCm, glLeftCenterCm, glRightCenterCm); delay(100); } if (abs(glFrontCm - Offset) > 1) { gl_pSerPort->printf("Second time through MTDFD with error = %d\n", abs(glFrontCm - Offset)); MoveToDesiredFrontDistCm(Offset,Kp/2,Ki,Kd); } |
This should have worked well, except when I tried it, the function exited with a ‘STUCK_AHEAD’ error – oops! Here’s a run going from 60 from 30cm:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 |
Moving Front to 30 cm, with PID = (1.50,0.10,0.20) Enter any key to start Got 51 Got 46 Got 46 Got 46 Got 255 Before MTDFD, FVar = 12228.16 MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20 MTFD: at start, tgt = 30cm, curr_dist = 65, front/rear var = 13328.0/833.0 Msec Fdist TgtD err Ival Derr Out Speed FVar RVar 4812697 64 30 -34.0 -3.4 -34.0 -47.6 -47 11618.7 23386.2 4812746 66 30 -36.0 -7.0 -2.0 -60.6 -60 11452.8 23271.3 4812800 65 30 -35.0 -10.5 1.0 -63.2 -63 11682.5 33698.9 4812849 64 30 -34.0 -13.9 1.0 -65.1 -65 12236.1 41759.9 4812899 63 30 -33.0 -17.2 1.0 -66.9 -66 13316.3 49390.2 4812944 60 30 -30.0 -20.2 3.0 -65.8 -65 14265.6 56593.1 4812981 61 30 -31.0 -23.3 -1.0 -69.6 -69 15163.5 63372.0 4813049 60 30 -30.0 -26.3 1.0 -71.5 -71 15891.4 69730.2 4813095 57 30 -27.0 -29.0 3.0 -70.1 -70 16326.1 77314.4 4813148 55 30 -25.0 -31.5 2.0 -69.4 -69 16138.6 84400.2 4813199 55 30 -25.0 -34.0 0.0 -71.5 -71 15176.2 90991.4 4813235 52 30 -22.0 -36.2 3.0 -69.8 -69 13311.0 97091.6 4813297 50 30 -20.0 -38.2 2.0 -68.6 -68 10317.8 101237.0 4813346 49 30 -19.0 -40.1 1.0 -68.8 -68 5972.6 104982.1 4813396 44 30 -14.0 -41.5 5.0 -63.5 -63 40.8 108330.4 MTFD: Stopped with front dist = 44 and errcode = STUCK_AHEAD |
The ‘stuck’ checks have to be there because the robot can’t ‘see’ obstacles that are too low to interrupt the front LIDAR beam, or aren’t directly in the line of sight, but how to manage? The ‘stuck’ checks depend on a variance calculation on the last 50 measurements (held in a bit-bucket array), so I began to wonder why the front variance was decreasing so rapidly while the rear variance wasn’t (one would think they would behave more or less the same). So I went back and printed out the front & back distance array contents after each run, and saw that the reason the front variance was decreasing so rapidly was because I was using a 50mSec time interval, meaning the 50-element array was getting filled in 2500mSec – or about 2.5sec. So, in order to get more variance in a normal run, either the run has to be done at a faster speed (leading to more overshoot) or the timing interval has to be increased. In earlier work I had discovered that the front LIDAR system produces errors for long distance measurements when using short measurement intervals, so I increased the measurement interval from 50 to 200mSec, and now the variance decreases at a much slower rate – yay!
With the longer 200mSec measurement interval, the movement routine now has time to adjust for overshoots, as shown below:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 |
Moving Front to 60 cm, with PID = (1.50,0.10,0.20) Enter any key to start Before MTDFD, FVar = 12843.11 MoveToDesiredFrontDistCm(60): Kp/Ki/Kd= 1.50/0.10/0.20 MTFD: at start, tgt = 60cm, curr_dist = 31, front/rear var = 13328.0/833.0 Msec Fdist TgtD err Ival Derr Out Speed FVar RVar 872999 29 60 31.0 3.1 31.0 43.4 43 12907.2 21444.6 873198 32 60 28.0 5.9 -3.0 48.5 48 13605.9 30074.4 873398 36 60 24.0 8.3 -4.0 45.1 45 14795.4 40126.6 873599 44 60 16.0 9.9 -8.0 35.5 35 16147.8 47845.2 873798 52 60 8.0 10.7 -8.0 24.3 24 17428.3 56907.6 873984 56 60 4.0 11.1 -4.0 17.9 17 18228.5 63729.1 874197 56 60 4.0 11.5 0.0 17.5 17 18902.0 71816.1 874401 57 60 3.0 11.8 -1.0 16.5 16 18813.2 77754.5 874602 59 60 1.0 11.9 -2.0 13.8 13 17558.3 84880.5 MTFD: Stopped with front dist = 59 and errcode = NO_ANOMALIES Front Rear Left Right 59 750.40 32.00 779.80 59 91.50 33.00 779.70 59 750.40 33.00 779.80 60 819.10 32.00 779.80 59 819.10 32.00 779.80 60 819.10 33.00 779.80 59 750.40 32.00 779.80 58 750.40 31.00 779.80 57 750.40 32.00 779.80 59 750.40 32.00 779.80 |
As the above run shows, the robot stopped very close (59cm) to the desired 60cm, without any significant overshoot, and the front variance actually increased significantly during the run – nice!
The following run was in the other direction – from 60 to 30cm:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 |
Moving Front to 30 cm, with PID = (1.50,0.10,0.20) Enter any key to start Before MTDFD, FVar = 17702.84 MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20 MTFD: at start, tgt = 30cm, curr_dist = 61, front/rear var = 13328.0/833.0 Msec Fdist TgtD err Ival Derr Out Speed FVar RVar 1164375 61 30 -31.0 -3.1 -31.0 -43.4 -43 11740.1 19499.2 1164572 59 30 -29.0 -6.0 2.0 -49.9 -49 11739.5 30074.4 1164772 54 30 -24.0 -8.4 5.0 -45.4 -45 12473.4 38267.3 1164975 51 30 -21.0 -10.5 3.0 -42.6 -42 13723.7 47845.2 1165173 44 30 -14.0 -11.9 7.0 -34.3 -34 15250.3 55133.9 1165363 37 30 -7.0 -12.6 7.0 -24.5 -24 16422.6 63729.1 1165575 32 30 -2.0 -12.8 5.0 -16.8 -16 17689.6 70127.6 1165774 30 30 0.0 -12.8 2.0 -13.2 -13 18190.7 76106.5 MTFD: Stopped with front dist = 30 and errcode = WALL_OFFSET_DIST_AHEAD Front Rear Left Right 30 750.40 29.00 779.80 30 750.40 29.00 779.80 30 750.40 29.00 779.80 31 819.10 29.00 779.80 32 819.10 28.00 779.80 32 819.10 29.00 779.80 31 819.10 29.00 779.80 28 819.10 29.00 779.80 32 819.10 28.00 779.80 28 750.40 28.00 779.80 Second time through MTDFD with error = 2 MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20 MTFD: at start, tgt = 30cm, curr_dist = 30, front/rear var = 13328.0/833.0 Msec Fdist TgtD err Ival Derr Out Speed FVar RVar MTFD: Stopped with front dist = 29 and errcode = OBSTACLE_AHEAD |
The robot did a very good job of stopping right on the mark, but coasted 2cm further over the next 2sec. This caused the test program to run the movement routine a second time, but it almost immediately exited again. The front/rear variance numbers were quite high, well out of the danger zone (the error codes shown did not effect the routine – it only looks for ‘STUCK_AHEAD’ and ‘STUCK_BEHIND’ which trigger on front and rear variance thresholds respectively.
So, for at least the ‘MoveToDesiredFrontDistCm()’ function, it appears that PID = (1.5, 0.1, 0.2) works very well. On to the next one!
MoveToDesiredRearDistCm():
Using the same PID set as for MoveToDesiredFrontDistCm() produces excellent results for the ‘rear motion’ routine as well. Here’s a run going from 60 to 30cm based on the rear distance sensor:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 |
Starting Front/BackSide Motion Test - enter Direction (1 = F, 2 = B, 3 = L, 4 = R), offset, Kp,Ki,Kd At top of parameter retrieval section DirStr = 2 OffStr = 30 Moving Back to 30 cm, with PID = (1.50,0.10,0.20) Enter any key to start In MoveToDesiredRearDistCm(30) with Kp/Ki/Kd = 1.5/0.1/0.2 MTRD: at start, tgt = 30cm, curr_dist = 57, front/rear var = 13328.0/833.0 Msec Fdist TgtD err Ival Derr Out Speed FVar RVar 27488 64.10 30 -34.1 -3.4 -34.1 -47.7 -47 10490.5 743.4 27685 57.20 30 -27.2 -6.1 6.9 -48.3 -48 9380.3 697.8 27889 58.80 30 -28.8 -9.0 -1.6 -51.9 -51 8803.9 653.9 28087 47.90 30 -17.9 -10.8 10.9 -39.8 -39 8602.4 612.5 28288 47.00 30 -17.0 -12.5 0.9 -38.2 -38 8804.5 573.6 28489 37.50 30 -7.5 -13.2 9.5 -26.4 -26 8982.6 542.8 28689 37.80 30 -7.8 -14.0 -0.3 -25.7 -25 9235.9 514.8 28889 30.60 30 -0.6 -14.1 7.2 -16.4 -16 9657.8 496.2 MTRD: Stopped with rear dist = 34 and errcode = NO_ANOMALIES Front Rear Left Right 120 29.10 824.00 27.30 119 28.30 824.00 27.10 118 28.80 824.00 27.40 117 29.00 824.00 26.90 121 32.60 824.00 27.30 118 32.50 824.00 27.30 120 32.10 824.00 27.90 119 32.00 824.00 27.30 121 28.80 824.00 27.30 119 28.80 824.00 27.20 |
MoveToDesiredLeftDistCm():
Next, I tackled the ‘MoveToDesiredLeftDistCm()’ function, which uses the left center distance measurement (corrected for orientation angle). This went fairly quickly, as the PID values for the front/back case seemed to work well for the ‘side’ case too. Here’s the output and a short video from a typical run:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 |
In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2 MTLD: at start, tgt = 50cm, curr_dist = 11.6, tgt_window = 2.0, front/rear var = 12424.4/795.3 Msec Left Steer LCorr TgtD err Ival Dval Out Speed FVar RVar 449355 15.0 0.4 11.0 50 35.0 3.5 35.0 49.0 49 11670.6 765.1 449555 17.0 0.4 13.6 50 33.0 6.8 -2.0 56.7 56 10361.3 733.2 449756 20.0 0.3 16.5 50 30.0 9.8 -3.0 55.4 55 8874.8 703.7 449967 24.0 0.4 17.6 50 26.0 12.4 -4.0 52.2 52 7277.8 671.1 450155 28.0 0.5 20.3 50 22.0 14.6 -4.0 48.4 48 5770.4 639.1 450358 32.0 0.3 26.4 50 18.0 16.4 -4.0 44.2 44 4821.3 606.5 450568 36.0 0.4 29.1 50 14.0 17.8 -4.0 39.6 39 3530.9 575.9 450757 40.0 0.4 29.8 50 10.0 18.8 -4.0 34.6 34 2451.5 548.3 450957 44.0 0.5 31.5 50 6.0 19.4 -4.0 29.2 29 1616.3 521.7 451158 49.0 0.3 41.3 50 1.0 19.5 -5.0 22.0 22 1034.0 499.6 451346 48.0 0.1 46.5 50 2.0 19.7 1.0 22.5 22 787.6 477.2 451558 51.0 0.4 40.8 50 -1.0 19.6 -3.0 18.7 18 584.6 460.0 451769 52.0 0.5 34.2 50 -2.0 19.4 -1.0 16.6 16 546.3 443.4 451958 52.0 0.4 42.0 50 -2.0 19.2 0.0 16.2 16 603.0 425.0 452160 54.0 0.5 38.7 50 -4.0 18.8 -2.0 13.2 13 679.0 410.9 452358 53.0 0.3 45.6 50 -3.0 18.5 1.0 13.8 13 688.5 397.6 452557 55.0 0.2 52.8 50 -5.0 18.0 -2.0 10.9 10 569.1 383.8 452759 55.0 0.4 43.5 50 -5.0 17.5 0.0 10.0 10 523.6 370.3 452960 54.0 0.3 46.0 50 -4.0 17.1 1.0 10.9 10 451.6 359.2 453161 54.0 0.2 51.8 50 -4.0 16.7 0.0 10.7 10 358.7 347.7 MTLD: Stopped with left dist = 51.8 and errcode = NO_ANOMALIES tLeft Steer LCorr 54.00 0.24 51.81 53.00 0.31 51.81 53.00 0.16 51.81 54.00 0.27 51.81 53.00 0.31 51.81 56.00 0.30 51.81 56.00 0.54 51.81 55.00 0.33 51.81 55.00 0.37 51.81 54.00 0.36 51.81 |
MoveToDesiredRightDistCm():
For this side, I simply copied ‘MoveToDesiredLeftDistCm()’ and changed all occurrences of ‘Left’ to ‘Right’. Here’s the telemetry and a short video from a typical run:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 |
In MoveToDesiredRightDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2 MTLD: at start, tgt = 50cm, curr_dist = 7.6, tgt_window = 2.0, front/rear var = 11089.8/793.5 Msec Right Steer LCorr TgtD err Ival Dval Out Speed FVar RVar 550636 12.5 0.6 8.0 50 37.5 3.8 37.5 52.5 52 9725.3 757.3 550836 12.6 0.5 8.2 50 37.4 7.5 -0.1 63.6 63 7898.5 721.0 551037 14.9 0.6 9.5 50 35.1 11.0 -2.3 64.1 64 6359.4 685.1 551236 18.5 0.6 11.5 50 31.5 14.1 -3.6 62.1 62 5139.8 650.9 551436 21.7 0.5 15.5 50 28.3 17.0 -3.2 60.1 60 4263.9 615.4 551638 25.7 0.6 16.2 50 24.3 19.4 -4.0 56.7 56 3856.9 582.3 551837 31.3 0.4 23.3 50 18.7 21.3 -5.6 50.5 50 3521.1 551.2 552037 36.1 0.5 25.2 50 13.9 22.7 -4.8 44.5 44 3467.5 521.7 552238 38.6 0.3 31.9 50 11.4 23.8 -2.5 41.4 41 3645.5 498.1 552438 43.1 0.5 30.4 50 6.9 24.5 -4.5 35.8 35 3949.4 472.4 552629 45.6 0.5 31.3 50 4.4 24.9 -2.5 32.0 32 4180.0 458.8 552838 48.1 0.7 24.3 50 1.9 25.1 -2.5 28.5 28 4478.5 442.0 553039 49.7 0.4 36.5 50 0.3 25.2 -1.6 25.9 25 4570.0 438.2 553239 50.6 0.5 34.3 50 -0.6 25.1 -0.9 24.4 24 4308.9 9713.3 553439 52.1 0.5 33.8 50 -2.1 24.9 -1.5 22.0 22 3548.7 9677.5 553628 54.0 0.3 47.3 50 -4.0 24.5 -1.9 18.9 18 2677.1 18508.9 553839 54.6 0.5 35.4 50 -4.6 24.0 -0.6 17.3 17 676.3 28807.2 554041 55.3 0.4 41.7 50 -5.3 23.5 -0.7 15.7 15 648.9 38619.4 554240 54.9 0.2 51.2 50 -4.9 23.0 0.4 15.6 15 588.1 46162.1 MTLD: Stopped with Right dist = 51.2 and errcode = NO_ANOMALIES tRight Steer LCorr 56.30 0.43 51.15 55.30 0.37 51.15 54.90 0.36 51.15 56.80 0.23 51.15 55.60 0.52 51.15 55.60 0.34 51.15 56.20 0.41 51.15 55.70 0.37 51.15 55.90 0.53 51.15 55.70 0.21 51.15 |
Summary:
It looks like all four (front/back/left/right) motion features are now working fine, with the same PID = (1.5, 0.1, 0.2) configuration – yay!
Now the challenge is to integrate this all back into my mainline program and start testing wall tracking again.
Stay Tuned,
Frank