Posted 01 December 2022
I thought I had the ‘parallel find’ problem solved about 18 months ago, back in April of 2021, but it seems this is a problem that refuses to die (well, up until now, anyway). This post describes the ‘new, new!’ solution, based on much improved distance measurement performance from the VL53L0X time-of-flight sensors and associated software. Basically, I discovered that I had been doing VL53L0X distance calibration all wrong, and a big part of the problem was my use of an integer instead of floating point data type to represent distance values. The VL53L0X sensor reports distances in integer millimeters, but when I converted the integer mm values to integer cm (without even rounding – but by truncation – yikes!) the resulting loss of precision caused significant problems with the parallel find algorithm.
So, after converting all my VL53L0X distance variables from integer to float (which turned out to be pretty easy as I had practiced good modularity and low coupling – yay!), I started over again with a two stage strategy. The first part addressed the calibration problem by redoing a series of tests to acquire compensation curves for both the right and left-side sensor arrays which were then programmed into the Teensy 3.5 processor that handles the VL53L0X arrays. The second part was to revisit the ‘parallel find’ algorithm. This post describes the result of the second part – revisiting the ‘parallel find’ algorithm.
The parallel find algorithm implements a two-stage search for the parallel condition, defined as the robot (or, more accurately, the relevant VL53L0X array) orientation that produces identical distance readings from the front and rear sensors of the relevant (i.e. left or right) array. The first ‘coarse’ stage searches for the change in sign of the steering value, and the second ‘fine tune’ stage searches for a steering value < 0.01, meaning that the front and rear distance values are nearly identical.
Here is the code for the parallel find subroutine:
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 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 |
bool RotateToParallelOrientation(WallTrackingCases trkdir, float Kp, float Ki, float Kd) { //Purpose: Rotate to parallel orientation (steering val == 0) // Inputs: // trkdir = WallTrackingCases enum denoting left or right-side tracking // ParallelFindKp,Ki,Kd = integers representing PID engine control params // Outputs: // Robot rotates CCW or CW to the desired orientation, under PID control // Plan: // Step1: rotate to parallel using sign-change criteria // Step2: fine-tune prallel using zero front/back distance criteria //Notes: // Created 05/24/22 for new IRHomingNavigateToIAP algorithm // 11/15/22 chg side distances to float vs uint16_t // 12/02/22 added kp,ki,kd to sig float lastError = 0; float lastInput = 0; float lastIval = 0; float lastDerror = 0; float front_d = 0; float rear_d = 0; float ParallelFindOutput = 0; //05/24/22 //DEBUG!! gl_pSerPort->printf("\nIn RotateToParallelOrientation(%s) with Kp/Ki/Kd = %2.1f/%2.1f/%2.1f\n", TrkStrArray[trkdir], Kp, Ki, Kd); //DEBUG!! StopBothMotors(); AnomalyCode errcode = NO_ANOMALIES; float steerval = 0; float prev_steerval = 0; gl_pSerPort->printf("Msec\tHdg\tFrontD\tRearD\tSteer\tPSteer\terr\tIval\tOut\tspeed\n"); //Step1: rotate to parallel using sign-change criteria elapsedMillis MsecSinceLastAdj = 0; //added 04/29/22 //while (errcode == NO_ANOMALIES && steerval * prev_steerval >= 0) while (steerval * prev_steerval >= 0) { CheckForUserInput(); if (MsecSinceLastAdj >= 200) { MsecSinceLastAdj -= 200; prev_steerval = steerval; UpdateAllEnvironmentParameters(); errcode = CheckForAnomalies(); steerval = (trkdir == TRACKING_LEFT) ? glLeftSteeringVal : glRightSteeringVal; front_d = (trkdir == TRACKING_LEFT) ? glLeftFrontCm : glRightFrontCm; rear_d = (trkdir == TRACKING_LEFT) ? glLeftRearCm : glRightRearCm; ParallelFindOutput = PIDCalcs(steerval, ParallelFindSetpoint, lastError, lastInput, lastIval, lastDerror, Kp, Ki, Kd); int16_t speed = (int16_t)ParallelFindOutput; //PIDCalcs out can be negative, must handle both signs speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed; speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed; UpdateIMUHdgValDeg(); //DEBUG!! gl_pSerPort->printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\n", millis(), IMUHdgValDeg, front_d, rear_d, steerval, prev_steerval, lastError, lastIval, ParallelFindOutput, speed); //DEBUG!! RunBothMotorsBidirectional(speed, -speed); //CCW for speed < 0, CW for speed > 0 } } gl_pSerPort->printf("Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = %2.1f/%2.1f/%2.1f/%s\n", IMUHdgValDeg, steerval, prev_steerval, AnomalyStrArray[errcode]); StopBothMotors(); delay(1000); //Step2: fine-tune prallel using zero front/back distance criteria //at this point, front & rear distances should be very close, so I should be //able to make them equal. gl_pSerPort->printf("Msec\tHdg\tFrontD\tRearD\tSteer\tPSteer\terr\tIval\tOut\tspeed\n"); MsecSinceLastAdj = 0; //added 04/29/22 while (abs(steerval) > 0.01f) { CheckForUserInput(); if (MsecSinceLastAdj >= 200) { MsecSinceLastAdj -= 200; prev_steerval = steerval; UpdateAllEnvironmentParameters(); errcode = CheckForAnomalies(); steerval = (trkdir == TRACKING_LEFT) ? glLeftSteeringVal : glRightSteeringVal; front_d = (trkdir == TRACKING_LEFT) ? glLeftFrontCm : glRightFrontCm; rear_d = (trkdir == TRACKING_LEFT) ? glLeftRearCm : glRightRearCm; ParallelFindOutput = PIDCalcs(steerval, ParallelFindSetpoint, lastError, lastInput, lastIval, lastDerror, ParallelFindKp, ParallelFindKi, ParallelFindKd); int16_t speed = (int16_t)ParallelFindOutput; //PIDCalcs out can be negative, must handle both signs speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed; speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed; //DEBUG!! gl_pSerPort->printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\n", millis(), IMUHdgValDeg, front_d, rear_d, steerval, prev_steerval, lastError, lastIval, ParallelFindOutput, speed); //DEBUG!! RunBothMotorsBidirectional(speed, -speed); //CCW for speed < 0, CW for speed > 0 } } gl_pSerPort->printf("Exited RTPO fine tune with hdg/frontD/steerval/prev_steerval = %2.1f/%d/%2.2f/%2.2f\n", IMUHdgValDeg, glFrontCm, steerval, prev_steerval); return true; } |
And here is a short video and the telemetry output for a successful parallel find 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 |
In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 100.0/10.0/0.0 Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 33033 20.8 28.0 23.0 0.4 0.0 -0.4 -4.3 -47.3 -47 33235 14.1 27.0 24.0 0.3 0.4 -0.3 -7.7 -41.7 -41 33435 1.7 24.0 22.0 0.2 0.3 -0.2 -9.8 -30.8 -30 33644 -7.8 24.0 22.0 0.2 0.2 -0.2 -11.9 -32.9 -32 33824 -13.7 21.0 22.0 -0.1 0.2 0.1 -11.1 -3.1 -3 Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -13.7/-0.1/0.2/NO_ANOMALIES Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 35057 -15.1 21.0 22.0 -0.1 -0.1 0.1 -9.9 2.1 2 35267 -15.1 20.0 23.0 -0.2 -0.1 0.2 -7.5 16.5 16 35467 -15.0 20.0 23.0 -0.2 -0.2 0.2 -5.1 18.9 18 35656 -14.7 21.0 23.0 -0.2 -0.2 0.2 -3.1 16.9 16 35854 -14.6 21.0 21.0 -0.1 -0.2 0.1 -2.6 2.4 2 36068 -14.6 21.0 21.0 -0.1 -0.1 0.1 -2.1 2.9 2 36258 -14.6 20.0 22.0 -0.1 -0.1 0.1 -1.0 10.0 10 36455 -14.6 20.0 22.0 -0.1 -0.1 0.1 0.1 11.1 11 36655 -14.6 20.0 22.0 -0.1 -0.1 0.1 1.4 14.4 14 36857 -14.6 20.0 22.0 -0.1 -0.1 0.1 2.7 15.7 15 37058 -14.6 20.0 22.0 -0.2 -0.1 0.2 4.6 23.6 23 37258 -14.2 20.0 20.0 0.0 -0.2 -0.0 4.2 0.2 0 37458 -14.0 20.0 20.0 0.0 0.0 -0.0 3.8 -0.2 0 37656 -14.0 21.0 22.0 -0.1 0.0 0.1 4.8 14.8 14 37860 -14.1 20.0 22.0 -0.2 -0.1 0.2 6.5 23.5 23 38060 -13.7 20.0 22.0 -0.2 -0.2 0.2 8.2 25.2 25 38258 -12.7 20.0 22.0 -0.1 -0.2 0.1 9.6 23.6 23 38460 -11.3 20.0 22.0 -0.1 -0.1 0.1 11.0 25.0 24 38659 -10.2 20.0 20.0 0.1 -0.1 -0.1 10.4 4.4 4 38860 -10.1 20.0 22.0 -0.1 0.1 0.1 11.7 24.7 24 39060 -10.1 20.0 22.0 -0.1 -0.1 0.1 13.0 26.0 25 39261 -10.0 20.0 20.0 0.0 -0.1 0.0 13.0 13.0 12 Exited RTPO fine tune with hdg/frontD/steerval/prev_steerval = -10.0/400/0.00/-0.13 39279 20.0 19.0 20.0 0.00 39401 20.0 19.0 20.0 0.00 39513 20.0 20.0 20.0 0.03 39636 20.0 20.0 20.0 0.03 |
So it appears that my original parallel find algorithm now performs much better, due mainly to the more accurate distance reporting obtained by changing from integer to float data type, and better distance compensation curves for each of the six sensors in the two VL53L0X arrays. Now I need to back-port this improved code into my main robot navigation code, probably by way of the Wall Track part-task program described in my earlier ‘More Wall Track PID Tuning‘ post.
29 December 2022 Update:
After going through some additional ‘part-task’ tune-up exercises, I started the process of integrating all the changes back into my Wall Tracking PID Tuning code. After the usual number of screw-ups I got the left-side offset capture feature working up to the point of turning back parallel to the wall. The code at this point just used ‘SpinTurn()’ to turn 30º in the opposite direction as the first turn away from the wall, with a note that this was done because ‘Parallel Find is fatally flawed’. Well, since I had just gotten through testing this feature I thought I could just drop it in. When I did, the robot promptly turned in the wrong direction and started spinning – yikes! Back to the ‘Parallel Find’ drawing board!
After making some code changes to make the part-task code a bit easier to use, I got everything working again, with basically the same PID values (100,0,0) as before. Here’s the output and a short video from a run:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
At top of parameter retrieval section Wallstr = 0 isRightWall = FALSE Parallel Find for Left Wall with PID = (100.0,0.0,0.0) Enter any key to start Got 48 Got 48 In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 100.0/0.0/0.0 Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 941942 109.2 36.0 30.0 0.6 0.0 -0.6 0.0 -63.0 -63 942142 97.7 31.0 27.0 0.4 0.6 -0.4 0.0 -38.0 -38 942311: Error in GetFrontDistCm() - 0 replaced with 312 942342 79.0 24.0 22.0 0.2 0.4 -0.2 0.0 -15.0 -15 942533 70.3 22.0 22.0 0.1 0.2 -0.1 0.0 -7.0 -7 942742 70.0 22.0 22.0 0.0 0.1 -0.0 0.0 -2.0 -2 942941 70.1 22.0 22.0 0.0 0.0 -0.0 0.0 -2.0 -2 943153 70.1 22.0 22.0 0.0 0.0 -0.0 0.0 -3.0 -3 943342 70.1 22.0 22.0 -0.0 0.0 0.0 0.0 2.0 2 Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 70.1/-0.0/0.0/NO_ANOMALIES |
30 December 2022 Update:
Not so fast, pardner! Well, it seems I was a bit premature about completing this effort – After a few more trials I realized this wasn’t working anywhere near as well as I thought – oops!
So, after lots more trials aimlessly wandering around PID space, I came up with new values that *seem* to work (for the left side case, at least). I also discovered that I really don’t need a two-stage process (‘coarse’ tune followed by ‘fine’ tune) to get a decent result, as long as the robot turns slowly enough to allow the distance measurement changes to keep up. Here’s are a couple of runs (telemetry output and short video) for the new setup.
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 |
In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0 Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 528906 -42.5 44.60 47.40 -0.28 0.00 0.28 1.12 6.72 6 529106 -42.5 44.70 46.20 -0.15 -0.28 0.15 1.72 4.72 4 529305 -42.5 45.30 46.40 -0.11 -0.15 0.11 2.16 4.36 4 529506 -42.5 43.50 47.30 -0.38 -0.11 0.38 3.68 11.28 11 529705 -42.5 43.50 47.00 -0.35 -0.38 0.35 5.08 12.08 12 529905 -42.5 44.10 46.50 -0.19 -0.35 0.19 5.84 9.64 9 530106 -42.5 43.80 45.90 -0.21 -0.19 0.21 6.68 10.88 10 530297 -42.5 44.60 47.30 -0.27 -0.21 0.27 7.76 13.16 13 530506 -42.5 43.70 46.30 -0.26 -0.27 0.26 8.80 14.00 14 530707 -42.5 44.20 47.00 -0.28 -0.26 0.28 9.92 15.52 15 530907 -42.5 44.50 47.70 -0.32 -0.28 0.32 11.20 17.60 17 531108 -42.5 44.30 47.60 -0.33 -0.32 0.33 12.52 19.12 19 531298 -41.7 43.90 46.30 -0.24 -0.33 0.24 13.48 18.28 18 531509 -41.4 43.40 45.70 -0.23 -0.24 0.23 14.40 19.00 19 531708 -41.4 43.60 44.50 -0.09 -0.23 0.09 14.76 16.56 16 531909 -41.4 44.40 46.10 -0.17 -0.09 0.17 15.44 18.84 18 532107 -41.4 44.20 46.20 -0.20 -0.17 0.20 16.24 20.24 20 532298 -41.4 44.00 47.00 -0.30 -0.20 0.30 17.44 23.44 23 532510 -41.3 43.20 46.90 -0.37 -0.30 0.37 18.92 26.32 26 532709 -39.7 43.40 45.40 -0.20 -0.37 0.20 19.72 23.72 23 532910 -37.4 41.60 43.30 -0.17 -0.20 0.17 20.40 23.80 23 533109 -35.3 39.90 41.90 -0.20 -0.17 0.20 21.20 25.20 25 533301 -33.4 39.70 41.90 -0.22 -0.20 0.22 22.08 26.48 26 533510 -31.7 38.60 40.20 -0.16 -0.22 0.16 22.72 25.92 25 533711 -30.1 37.20 37.90 -0.07 -0.16 0.07 23.00 24.40 24 533900 -28.4 37.00 38.40 -0.14 -0.07 0.14 23.56 26.36 26 534111 -26.6 36.10 37.30 -0.12 -0.14 0.12 24.04 26.44 26 534302 -25.2 35.90 36.30 -0.04 -0.12 0.04 24.20 25.00 24 534511 -23.6 35.00 36.40 -0.14 -0.04 0.14 24.76 27.56 27 534710 -21.7 34.10 35.20 -0.11 -0.14 0.11 25.20 27.40 27 534912 -19.6 33.90 34.70 -0.08 -0.11 0.08 25.52 27.12 27 535111 -17.3 33.40 33.30 0.01 -0.08 -0.01 25.48 25.28 25 Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -17.29/0.01/-0.08/NO_ANOMALIES 536138 33.7 34.6 33.8 -0.01 536247 32.9 33.7 33.6 -0.07 536365 33.4 33.9 33.6 -0.02 536475 33.4 34.5 33.4 0.00 536591 33.5 33.5 33.5 0.00 536702 33.6 33.7 33.6 0.00 536820 33.1 34.2 33.3 -0.02 536928 33.5 34.3 33.6 -0.01 537038 33.0 34.1 33.8 -0.08 537156 32.9 33.6 33.8 -0.08 |
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 |
In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0 Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 623308 31.3 42.40 37.10 0.53 0.00 -0.53 -2.12 -12.72 -12 623498 31.3 42.80 37.50 0.53 0.53 -0.53 -4.24 -14.84 -14 623697 31.2 42.00 37.30 0.47 0.53 -0.47 -6.12 -15.52 -15 623897 31.1 42.40 36.90 0.55 0.47 -0.55 -8.32 -19.32 -19 624099 31.0 41.30 37.10 0.42 0.55 -0.42 -10.00 -18.40 -18 624297 31.0 41.30 37.40 0.39 0.42 -0.39 -11.56 -19.36 -19 624497 31.0 41.00 37.20 0.38 0.39 -0.38 -13.08 -20.68 -20 624699 30.9 42.80 37.20 0.49 0.38 -0.49 -15.04 -24.84 -24 624899 30.4 42.80 36.20 0.66 0.49 -0.66 -17.68 -30.88 -30 625098 26.4 40.40 36.10 0.43 0.66 -0.43 -19.40 -28.00 -28 625300 20.9 38.10 33.80 0.43 0.43 -0.43 -21.12 -29.72 -29 625510 15.1 36.30 33.30 0.30 0.43 -0.30 -22.32 -28.32 -28 625701 10.6 35.50 32.40 0.31 0.30 -0.31 -23.56 -29.76 -29 625900 5.1 34.10 32.10 0.20 0.31 -0.20 -24.36 -28.36 -28 626100 0.7 34.30 32.10 0.22 0.20 -0.22 -25.24 -29.64 -29 626290 -3.1 31.70 32.20 -0.05 0.22 0.05 -25.04 -24.04 -24 Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -3.07/-0.05/0.22/NO_ANOMALIES 627317 32.3 35.1 30.8 0.15 627430 32.1 34.8 31.5 0.06 627538 32.1 34.2 31.0 0.11 627654 32.2 33.9 31.1 0.11 627762 32.6 34.7 31.5 0.11 627879 32.0 34.9 30.9 0.11 627987 32.4 35.3 31.1 0.13 628101 32.7 34.6 31.5 0.12 628214 32.9 35.4 31.5 0.14 628328 32.5 34.9 31.6 0.09 |
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 |
In RotateToParallelOrientation(Right) with Kp/Ki/Kd = 20.0/4.0/0.0 Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 134663 55.1 46.10 50.90 -0.48 0.00 0.48 1.92 11.52 11 134875 55.1 45.40 51.20 -0.58 -0.48 0.58 4.24 15.84 15 135064 55.1 45.60 53.00 -0.74 -0.58 0.74 7.20 22.00 22 135266 53.8 44.70 52.00 -0.73 -0.74 0.73 10.12 24.72 24 135465 51.0 43.90 49.10 -0.52 -0.73 0.52 12.20 22.60 22 135666 48.6 41.30 46.20 -0.49 -0.52 0.49 14.16 23.96 23 135864 46.3 40.00 43.90 -0.39 -0.49 0.39 15.72 23.52 23 136066 44.2 39.00 43.70 -0.47 -0.39 0.47 17.60 27.00 26 136266 41.2 37.60 40.60 -0.30 -0.47 0.30 18.80 24.80 24 136466 37.6 36.80 39.30 -0.25 -0.30 0.25 19.80 24.80 24 136677 33.6 35.60 38.20 -0.26 -0.25 0.26 20.84 26.04 26 136867 30.0 34.60 35.50 -0.09 -0.26 0.09 21.20 23.00 23 137066 26.7 33.70 34.10 -0.04 -0.09 0.04 21.36 22.16 22 137267 24.4 33.60 34.50 -0.09 -0.04 0.09 21.72 23.52 23 137467 22.3 33.20 33.10 0.01 -0.09 -0.01 21.68 21.48 21 Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 22.29/0.01/-0.09/NO_ANOMALIES 138486 33.4 32.7 33.5 -0.01 138600 33.2 32.7 33.6 -0.04 138708 33.2 32.3 33.7 -0.05 138822 33.0 32.6 33.3 -0.03 138926 32.9 32.3 32.5 0.04 139035 33.3 31.9 33.8 -0.05 139146 32.8 32.4 33.5 -0.07 139254 33.1 32.6 33.1 0.00 139367 33.0 32.5 33.8 -0.08 139474 32.7 32.2 33.9 -0.12 |
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 |
Starting Parallel Find Test - enter Wall (1 = right/0 = left), PID triplet Kp,Ki,Kd At top of parameter retrieval section Wallstr = 1 isRightWall = TRUE Parallel Find for Right Wall with PID = (20.0,4.0,0.0) Enter any key to start Got 44 Got 10 In RotateToParallelOrientation(Right) with Kp/Ki/Kd = 20.0/4.0/0.0 Msec Hdg FrontD RearD Steer PSteer err Ival Out speed 168221 -14.9 47.00 42.10 0.49 0.00 -0.49 -1.96 -11.76 -11 168421 -14.9 47.10 41.50 0.56 0.49 -0.56 -4.20 -15.40 -15 168622 -14.9 46.80 40.30 0.65 0.56 -0.65 -6.80 -19.80 -19 168812 -14.8 47.00 42.30 0.47 0.65 -0.47 -8.68 -18.08 -18 169011 -14.4 46.90 40.30 0.66 0.47 -0.66 -11.32 -24.52 -24 169212 -13.5 46.80 40.40 0.64 0.66 -0.64 -13.88 -26.68 -26 169412 -11.0 45.70 41.00 0.47 0.64 -0.47 -15.76 -25.16 -25 169611 -8.5 44.30 39.60 0.47 0.47 -0.47 -17.64 -27.04 -27 169812 -5.5 43.60 38.90 0.47 0.47 -0.47 -19.52 -28.92 -28 170013 -1.7 42.30 37.60 0.47 0.47 -0.47 -21.40 -30.80 -30 170213 2.7 40.70 38.90 0.23 0.47 -0.23 -22.32 -26.92 -26 170413 6.6 39.60 37.80 0.18 0.23 -0.18 -23.04 -26.64 -26 170613 9.6 38.60 37.80 0.08 0.18 -0.08 -23.36 -24.96 -24 170812 11.8 38.60 36.10 0.25 0.08 -0.25 -24.36 -29.36 -29 171013 15.0 38.60 38.50 0.01 0.25 -0.01 -24.40 -24.60 -24 171214 17.3 38.60 37.90 0.07 0.01 -0.07 -24.68 -26.08 -26 171414 19.0 37.90 38.70 -0.08 0.07 0.08 -24.36 -22.76 -22 Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 19.01/-0.08/0.07/NO_ANOMALIES 172430 37.9 37.6 38.3 -0.04 172535 38.3 37.0 38.5 -0.02 172642 38.3 37.7 38.3 0.00 172748 38.0 37.7 37.6 0.04 172855 38.3 37.4 38.4 -0.01 172962 38.3 37.7 37.8 0.05 173071 37.8 37.9 37.3 0.05 173175 38.1 37.5 37.5 0.06 173282 37.6 37.8 39.6 -0.20 173391 38.3 37.3 38.7 -0.04 |
05 January 2023 Update:
Well, even the above ‘slow as she goes’ idea doesn’t work all the time or that well. I wound up trying to instrument what is going on by doing the following
- Turning the robot in 10⁰ steps, followed by a 1500 mSec pause to let the measurements catch up
- At 100mSec intervals during the 1500mSec pause, computing and displaying the 5-pt running average of the left front and rear distances, along with the running average computation of the steering value.
When I did this, I got the following plot
As can be seen from the above plots, there is significant variation over the 1500mSec interval, even though the robot is stationary. In particular, the last plot shows that at the start of the 1500mSec stationary period, the steering value goes from 0.15 (i.e. not parallel at all) to near zero (i.e. very parallel), even though the robot isn’t moving at all.
I have no idea why this is happening, but it sure screws up any thoughts of rapidly finding a parallel orientation, and even sheds significant doubt on the entire idea of using multiple VL53L0X sensors for wall tracking – UGH!!
This time I tried a ‘parallel find’ run using 5⁰ SpinTurn() steps, with no averaging. Here’s the data, and a short video showing the result
This actually looks pretty good, and the unaveraged distance sensor results, although noisy, behaved reasonably well.
07 January 2023 Update:
As I was drifting off to sleep After yesterday’s ‘successful’ trial runs, I was happy visualizing the robot using the ‘SpinTurn() facility to gradually turn to the (mostly) parallel position (I’m a visual person, so I often turn programming problems into visual ones), it suddenly struck me that I was taking the long way around the barn; here I was sneaking up on the parallel orientation by small SpinTurn increments waiting for the steering value to change signs, when actually all I had to do was use my ‘GetWallOrientDeg(float steerval)’. This function takes the current steering value as the sole parameter and returns the current orientation angle with respect to the nearest wall; with this information I could use SpinTurn() to rotate to the parallel orientation in one shot – cool!
So, I recoded my test program to do just that, with the ‘enhancement’ (I hope) of taking a second or so at the start to acquire a 10-point average of the steering value, so as to, hopefully, reduce errors due to noisy distance sensor outputs. Here’s the output and a short video showing the result:
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 |
40.20 36.00 40.60 35.90 40.00 35.40 40.50 35.20 40.10 35.10 40.70 36.00 40.00 34.90 39.60 35.50 40.20 35.00 40.30 35.70 starting front/rear/steer avgs = 40.22/35.47/0.47 GetWallOrientDeg(0.47) returned starting orientation of 27.14 deg Init hdg = 39.23 deg, Turn = 27.14 deg, tgt = 12.09 deg, timeout = 1.81 sec 32.20 30.30 31.30 29.70 30.90 29.90 31.10 30.20 31.40 30.20 31.40 29.70 31.20 29.90 30.60 29.90 30.80 29.70 31.10 29.20 ending front/rear/steer avgs = 31.20/29.87/0.13 GetWallOrientDeg(0.13) returned ending orientation of 7.60 deg |
This seemed to work very well, and is a much simpler algorithm than trying to use an ‘on the fly’ steering value and a PID machine. I believe with this result I can get back to the problem I was originally trying to solve – that of reasonable wall following.
02 February 2023 Update:
Referring to my current quest to incorporate the results from all my previous ‘part-task’ programs into a ‘WallE3_Complete_V1’, I have been going through each ‘part-task’ program to verify results, and then incorporating the results (usually in the form of a single function) into the ‘complete’ program. When I got to WallE3_ParallelFind_V1, I discovered that, although I had made quite a bit of progress, including drastically simplifying the ‘parallel find’ algorithm, it still didn’t work very well.
After some additional work, I now think that ‘parallel find’ is ready for inclusion in the complete program. Here’s the functional code from WallE3_ParallelFind_V1:
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 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 |
//Step 0: set up arrays for 5-point running average calcs const uint16_t DIST_ARRAY_SIZE = 10;//01/07/23 float frontdistsum = 0; float reardistsum = 0; if (isRightWall) { //01/07/22 now using initial average steering value for single-turn solution for (size_t i = 0; i < DIST_ARRAY_SIZE; i++) { GetRequestedVL53l0xValues(VL53L0X_RIGHT); frontdistsum += glRightFrontCm; reardistsum += glRightRearCm; delay(200); gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm); } float avgfrontdist = frontdistsum / DIST_ARRAY_SIZE; float avgreardist = reardistsum / DIST_ARRAY_SIZE; float avgsteerval = (avgfrontdist - avgreardist) / 10.f; gl_pSerPort->printf("starting front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n", avgfrontdist, avgreardist, avgsteerval); //01/07/23 now use average steering value to determine orientation w/r/t near wall float orientdeg = GetWallOrientDeg(avgsteerval); gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned starting orientation of %2.2f deg\n", avgsteerval, orientdeg); //modify turn rate based on size of rotation angle float dps = 15;//default rate for small rotation angles if (abs(orientdeg) > 15) { //dps = orientdeg; //basically a 1 second turn dps = abs(orientdeg); //basically a 1 second turn } orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, dps) : SpinTurn(isRightWall, abs(orientdeg), dps); //01/07/22 do another 10-pt average to see how we did frontdistsum = 0; reardistsum = 0; for (size_t i = 0; i < DIST_ARRAY_SIZE; i++) { GetRequestedVL53l0xValues(VL53L0X_RIGHT); frontdistsum += glRightFrontCm; reardistsum += glRightRearCm; delay(200); gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm); } avgfrontdist = frontdistsum / DIST_ARRAY_SIZE; avgreardist = reardistsum / DIST_ARRAY_SIZE; avgsteerval = (avgfrontdist - avgreardist) / 10.f; gl_pSerPort->printf("ending front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n", avgfrontdist, avgreardist, avgsteerval); orientdeg = GetWallOrientDeg(avgsteerval); gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned ending orientation of %2.2f deg\n", avgsteerval, orientdeg); //01/07/23 try another turn if necessary if (abs(orientdeg) > 5) { orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, 30) : SpinTurn(isRightWall, abs(orientdeg), dps); } //01/07/22 do another 10-pt average to see how we did frontdistsum = 0; reardistsum = 0; for (size_t i = 0; i < DIST_ARRAY_SIZE; i++) { GetRequestedVL53l0xValues(VL53L0X_RIGHT); frontdistsum += glRightFrontCm; reardistsum += glRightRearCm; delay(200); gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm); } avgfrontdist = frontdistsum / DIST_ARRAY_SIZE; avgreardist = reardistsum / DIST_ARRAY_SIZE; avgsteerval = (avgfrontdist - avgreardist) / 10.f; gl_pSerPort->printf("after second turn: front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n", avgfrontdist, avgreardist, avgsteerval); } else //wall must be on left side { //01/07/22 now using initial average steering value for single-turn solution for (size_t i = 0; i < DIST_ARRAY_SIZE; i++) { GetRequestedVL53l0xValues(VL53L0X_LEFT); frontdistsum += glLeftFrontCm; reardistsum += glLeftRearCm; delay(200); gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm); } float avgfrontdist = frontdistsum / DIST_ARRAY_SIZE; float avgreardist = reardistsum / DIST_ARRAY_SIZE; float avgsteerval = (avgfrontdist - avgreardist) / 10.f; gl_pSerPort->printf("starting front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n", avgfrontdist, avgreardist, avgsteerval); //01/07/23 now use average steering value to determine orientation w/r/t near wall float orientdeg = GetWallOrientDeg(avgsteerval); gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned starting orientation of %2.2f deg\n", avgsteerval, orientdeg); //modify turn rate based on size of rotation angle float dps = 15;//default rate for small rotation angles if (abs(orientdeg) > 15) { //dps = orientdeg; //basically a 1 second turn dps = abs(orientdeg); //basically a 1 second turn } orientdeg > 0? SpinTurn(!isRightWall, orientdeg, dps): SpinTurn(isRightWall, abs(orientdeg), dps); //01/07/22 do another 10-pt average to see how we did frontdistsum = 0; reardistsum = 0; for (size_t i = 0; i < DIST_ARRAY_SIZE; i++) { GetRequestedVL53l0xValues(VL53L0X_LEFT); frontdistsum += glLeftFrontCm; reardistsum += glLeftRearCm; delay(200); gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm); } avgfrontdist = frontdistsum / DIST_ARRAY_SIZE; avgreardist = reardistsum / DIST_ARRAY_SIZE; avgsteerval = (avgfrontdist - avgreardist) / 10.f; gl_pSerPort->printf("ending front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n", avgfrontdist, avgreardist, avgsteerval); orientdeg = GetWallOrientDeg(avgsteerval); gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned ending orientation of %2.2f deg\n", avgsteerval, orientdeg); //01/07/23 try another turn if necessary if (abs(orientdeg) > 5) { orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, 30) : SpinTurn(isRightWall, abs(orientdeg), dps); } //01/07/22 do another 10-pt average to see how we did frontdistsum = 0; reardistsum = 0; for (size_t i = 0; i < DIST_ARRAY_SIZE; i++) { GetRequestedVL53l0xValues(VL53L0X_LEFT); frontdistsum += glLeftFrontCm; reardistsum += glLeftRearCm; delay(200); gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm); } avgfrontdist = frontdistsum / DIST_ARRAY_SIZE; avgreardist = reardistsum / DIST_ARRAY_SIZE; avgsteerval = (avgfrontdist - avgreardist) / 10.f; gl_pSerPort->printf("after second turn: front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n", avgfrontdist, avgreardist, avgsteerval); } |
The above code will replace the code in ‘RotateToParallelOrientation()’. See my post on consolidating everything into a ‘Complete’ program for more details
Stay tuned,
Frank
Pingback: WallE3 Rolling Turn, Revisited | Paynter's Palace
Pingback: WallE3 Wall Tracking, Revisited | Paynter's Palace