Posted 14 November 2023
After getting the charging station (re)integrated with the rest of the system, I have been working on complete travel-charge-travel charge cycles, where WallE3 travels around the house, finds and connects to the charger, disconnects, travels around the house some more, and then finds its way back to the charger – lather, rinse, repeat.
However, in the process I have run into a problem with the MoveToDesired(Front|Back|Left|Right)Distance() function. On several occasions the robot has blown right by the desired distance and run headlong (or backlong?) into a wall. Investigating has led me to realize that the cause of this problem is the infamous ‘integral windup’ characteristic inherent in insufficiently sophisticated PID algorithms. Here’s the relevant telemetry from a recent MoveToDesiredFrontDistance() run, and an Excel plot showing the ‘integral windup’ issue.
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 |
MTFD: at start, tgt = 30cm, curr_dist = 250, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 87888 251 30 -221.0 -22.1 -309.4 -75 74893 12608 87938 248 30 -218.0 -43.9 -371.5 -75 72136 12252 87988 251 30 -221.0 -66.0 -396.9 -75 69678 11896 88038 250 30 -220.0 -88.0 -418.2 -75 67551 11545 88088 250 30 -220.0 -110.0 -440.0 -75 65737 11201 88138 245 30 -215.0 -131.5 -455.0 -75 64281 10861 88188 247 30 -217.0 -153.2 -478.3 -75 63096 10526 88238 242 30 -212.0 -174.4 -493.4 -75 62249 10198 88288 241 30 -211.0 -195.5 -512.2 -75 61683 9874 88337 238 30 -208.0 -216.3 -528.9 -75 61406 9556 88387 233 30 -203.0 -236.6 -542.1 -75 61429 9245 88437 234 30 -204.0 -257.0 -562.8 -75 61665 8939 88488 233 30 -203.0 -277.3 -582.0 -75 62121 8638 88987 212 30 -182.0 -468.0 -740.8 -75 74994 5985 89037 207 30 -177.0 -485.7 -752.2 -75 76588 5753 89087 206 30 -176.0 -503.3 -767.5 -75 78107 5527 89137 204 30 -174.0 -520.7 -782.1 -75 79531 5308 89187 205 30 -175.0 -538.2 -800.5 -75 80794 5095 89237 201 30 -171.0 -555.3 -812.6 -75 81915 4889 89287 199 30 -169.0 -572.2 -826.1 -75 82838 4688 89337 196 30 -166.0 -588.8 -838.4 -75 83536 4492 89387 194 30 -164.0 -605.2 -851.6 -75 83959 4301 89437 190 30 -160.0 -621.2 -862.0 -75 84089 4119 89487 188 30 -158.0 -637.0 -874.4 -75 83864 3944 89537 187 30 -157.0 -652.7 -888.4 -75 83234 3777 89589 185 30 -155.0 -668.2 -901.1 -75 82166 3614 89637 183 30 -153.0 -683.5 -913.4 -75 80616 3456 89687 183 30 -153.0 -698.8 -928.3 -75 78523 3304 89739 181 30 -151.0 -713.9 -940.8 -75 75860 3153 89789 177 30 -147.0 -728.6 -949.9 -75 72591 3013 89839 175 30 -145.0 -743.1 -961.0 -75 68654 2878 89889 176 30 -146.0 -757.7 -976.5 -75 63981 2750 89939 173 30 -143.0 -772.0 -987.1 -75 58547 2625 89989 173 30 -143.0 -786.3 -1000.8 -75 52284 2507 90039 170 30 -140.0 -800.3 -1010.9 -75 45155 2388 90089 165 30 -135.0 -813.8 -1017.3 -75 37114 2282 90139 165 30 -135.0 -827.3 -1029.8 -75 28081 2180 90189 162 30 -132.0 -840.5 -1039.1 -75 18014 2083 90239 159 30 -129.0 -853.4 -1047.5 -75 6852 1987 90289 157 30 -127.0 -866.1 -1057.0 -75 850 1902 90339 156 30 -126.0 -878.7 -1067.9 -75 863 1826 90389 153 30 -123.0 -891.0 -1076.1 -75 874 1743 90439 151 30 -121.0 -903.1 -1085.0 -75 887 1668 90489 150 30 -120.0 -915.1 -1095.3 -75 888 1605 90539 146 30 -116.0 -926.7 -1101.5 -75 892 1549 90589 142 30 -112.0 -937.9 -1106.7 -75 894 1497 90639 139 30 -109.0 -948.8 -1112.9 -75 905 1441 90689 139 30 -109.0 -959.7 -1123.2 -75 902 1386 90739 137 30 -107.0 -970.4 -1131.3 -75 906 1340 90789 137 30 -107.0 -981.1 -1141.6 -75 902 1295 90839 134 30 -104.0 -991.5 -1148.1 -75 902 1255 90889 133 30 -103.0 -1001.8 -1156.5 -75 907 1219 90939 130 30 -100.0 -1011.8 -1162.4 -75 907 1189 90989 127 30 -97.0 -1021.5 -1167.6 -75 906 1162 91039 126 30 -96.0 -1031.1 -1175.3 -75 905 1140 91089 123 30 -93.0 -1040.4 -1180.5 -75 906 1120 91139 121 30 -91.0 -1049.5 -1186.4 -75 909 1109 91189 119 30 -89.0 -1058.4 -1192.3 -75 909 1093 91239 118 30 -88.0 -1067.2 -1199.4 -75 909 1093 91489 108 30 -78.0 -1108.2 -1225.4 -75 894 1096 91539 106 30 -76.0 -1115.8 -1230.2 -75 896 1101 91589 103 30 -73.0 -1123.1 -1233.2 -75 898 1108 91639 102 30 -72.0 -1130.3 -1238.5 -75 898 1112 91689 99 30 -69.0 -1137.2 -1241.3 -75 893 1117 91739 97 30 -67.0 -1143.9 -1244.8 -75 892 1121 91792 95 30 -65.0 -1150.4 -1248.3 -75 891 1122 91839 92 30 -62.0 -1156.6 -1250.2 -75 893 1121 91891 90 30 -60.0 -1162.6 -1253.0 -75 896 1120 91942 89 30 -59.0 -1168.5 -1257.2 -75 900 1118 91992 88 30 -58.0 -1174.3 -1261.5 -75 901 1120 92042 87 30 -57.0 -1180.0 -1265.7 -75 899 1116 92092 83 30 -53.0 -1185.3 -1265.6 -75 901 1106 92142 81 30 -51.0 -1190.4 -1267.3 -75 903 1098 92192 79 30 -49.0 -1195.3 -1269.2 -75 900 1082 92242 76 30 -46.0 -1199.9 -1269.5 -75 899 1107 92292 75 30 -45.0 -1204.4 -1272.1 -75 900 1089 92342 75 30 -45.0 -1208.9 -1276.4 -75 896 1062 92392 71 30 -41.0 -1213.0 -1275.3 -75 890 1038 92442 69 30 -39.0 -1216.9 -1275.8 -75 886 998 92492 67 30 -37.0 -1220.6 -1276.5 -75 878 965 92542 66 30 -36.0 -1224.2 -1278.4 -75 868 910 92592 64 30 -34.0 -1227.6 -1279.0 -75 866 851 92642 61 30 -31.0 -1230.7 -1277.8 -75 860 782 92692 60 30 -30.0 -1233.7 -1278.9 -75 855 702 92741 57 30 -27.0 -1236.4 -1277.5 -75 853 637 92791 56 30 -26.0 -1239.0 -1278.2 -75 849 557 92841 52 30 -22.0 -1241.2 -1275.0 -75 848 554 92892 51 30 -21.0 -1243.3 -1275.0 -75 845 545 92942 47 30 -17.0 -1245.0 -1271.3 -75 847 566 92992 45 30 -15.0 -1246.5 -1269.4 -75 846 556 93041 43 30 -13.0 -1247.8 -1267.7 -75 850 545 93091 40 30 -10.0 -1248.8 -1264.4 -75 859 544 93142 38 30 -8.0 -1249.6 -1262.0 -75 869 557 93191 35 30 -5.0 -1250.1 -1258.2 -75 878 554 93242 34 30 -4.0 -1250.5 -1256.7 -75 885 552 93292 33 30 -3.0 -1250.8 -1255.5 -75 884 548 93342 31 30 -1.0 -1250.9 -1252.8 -75 886 541 MTFD: Stopped with front dist = 31, anomaly code = NONE |
In the above Excel plot, the initial error is -221, which causes an output of ~300. The motor speed is clamped to -75, so the distance and error start slowly heading toward the target of 30cm and an error of zero. However, the integral (I) term continues to increase from near zero to well over -1000, and the output term was completely dominated by the integral value, keeping the motor speed clamped at -75 even as the measured distance approached and passed the target. In this particular case, I got lucky as the actual distance and the target distance were within the +/- 1cm termination window and the loop terminated. In other cases where the actual distance went through the termination window too rapidly, the robot would basically continued forever – or at least as long as necessary to ‘unwind’ the integral term.
So, what to do? Reading up on ‘integral windup’, I found this article from around 1990 (judging by dates on the references), and I decided to try the method described there as the ‘back-calculation and tracking’ method. The idea is that when the output gets clamped to some maximum value (+/- 75 in this application), the integral value is recomputed to a value that would produce the output that would naturally produce the clamp value. For instance for an error value of -221, the Ival is -22.1 which results in an output of -309, which gets clamped to -75. For an output of -75 with an input error of -221 we have -221 + Ival*(-221) + (-221*1.5) = -75 –> Ival = (-75 + 331.5)/-221 = -1.16. Checking, -1.16*(-221) = 256.36, 1.5*(-221) = 331.5, 256.36 – 331.5 = -75.14.
After thinking about this some more, I wondered if instead of back-calculating a new I value, maybe I could simply zero out the retained ‘last_Ival’ parameter whenever the PID output value is high enough so that it would get clamped. Assuming the same clamping value of 75 and a ‘P’ value of 1.5, this would happen any time the absolute value of the error value is greater or equal to 75/1.5 = 50.
Using Excel to apply this algorithm to the telemetry above telemetry data, I get the following plot.
In the above plot, the integral term (yellow line) was modified to be exactly zero whenever the output would have been > 75 or < -75 (the motor speed clamping values), even with a zero I value. As can be seen above, it stays zero until point 75. After that it smoothly decreases to about -55.6 at point 99, and then smoothly increases to about 30.98 at point 135, at which point is gets clamped to zero again. The motor speed value (green line) stays at -75 to point 75, at which point it smoothly (and linearly it appears) increases to +75, where it is clamped again.
I believe this might just do the trick. It will certainly prevent infinite runaway if the error term goes through zero too quickly to cause the loop to exit, as the robot will stop and then back up to the target distance.
I *think* I can modify my ‘PIDCalcs()’ function, and then everything that uses it will get the benefit of the ‘non-winding integral term’ algorithm.
17 November 2023 Update:
One day to go until SpaceX makes its second attempt at getting the world’s largest rocket into space – yeah!
To work the ‘integral term windup’ issue, I ported the FrontBackMotionTest code from an earlier program into the WallE3_Quicksort_V4 project so I could iterate easier, and got that running. As a baseline, here is the telemetry and a short video from the first 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 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 |
MTFD: at start, tgt = 30cm, curr_dist = 222, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 435737 214 30 -184.0 -18.4 -257.6 -75 75750 12960 435787 218 30 -188.0 -37.2 -318.4 -75 73337 12797 435837 212 30 -182.0 -55.4 -329.6 -75 71330 12648 435887 216 30 -186.0 -74.0 -352.2 -75 69603 12511 435937 212 30 -182.0 -92.2 -366.0 -75 68242 12385 435989 209 30 -179.0 -110.1 -379.2 -75 67223 12268 436039 210 30 -180.0 -128.1 -397.9 -75 66486 12158 436089 207 30 -177.0 -145.8 -411.9 -75 66066 12060 436139 209 30 -179.0 -163.7 -431.8 -75 65889 11969 436189 203 30 -173.0 -181.0 -441.7 -75 66037 11886 436239 201 30 -171.0 -198.1 -455.0 -75 66445 11811 436289 203 30 -173.0 -215.4 -474.5 -75 67044 11739 436339 198 30 -168.0 -232.2 -485.2 -75 67904 11675 436389 193 30 -163.0 -248.5 -494.0 -75 69007 11618 436439 196 30 -166.0 -265.1 -513.5 -75 70227 11567 436489 190 30 -160.0 -281.1 -522.3 -75 71659 11515 436539 184 30 -154.0 -296.5 -528.7 -75 73278 11467 436589 187 30 -157.0 -312.2 -547.1 -75 74945 11425 436639 180 30 -150.0 -327.2 -553.6 -75 76760 11386 436689 182 30 -152.0 -342.4 -570.0 -75 78583 11354 436739 179 30 -149.0 -357.3 -581.4 -75 80449 11326 436789 176 30 -146.0 -371.9 -591.5 -75 82328 11302 436838 174 30 -144.0 -386.3 -602.7 -75 84178 11282 436889 170 30 -140.0 -400.3 -611.1 -75 85989 11267 436938 169 30 -139.0 -414.2 -622.9 -75 87693 11257 436988 169 30 -139.0 -428.1 -636.6 -75 89245 11249 437038 165 30 -135.0 -441.6 -644.9 -75 90656 11245 437091 163 30 -133.0 -454.9 -654.8 -75 91866 11244 437141 160 30 -130.0 -467.9 -663.5 -75 92848 11245 437191 159 30 -129.0 -480.8 -674.5 -75 93542 11251 437241 155 30 -125.0 -493.3 -681.6 -75 93940 11254 437291 151 30 -121.0 -505.4 -687.7 -75 93997 11262 437341 145 30 -115.0 -516.9 -690.6 -75 93690 11272 437391 145 30 -115.0 -528.4 -700.9 -75 92914 11286 437441 145 30 -115.0 -539.9 -712.4 -75 91626 11301 437491 139 30 -109.0 -550.8 -715.5 -75 89835 11319 437541 137 30 -107.0 -561.5 -722.4 -75 87455 11336 437591 133 30 -103.0 -571.8 -727.1 -75 84453 11357 437640 133 30 -103.0 -582.1 -736.6 -75 80748 11377 437691 131 30 -101.0 -592.2 -744.1 -75 76306 11399 437740 126 30 -96.0 -601.8 -746.8 -75 71093 11421 437790 127 30 -97.0 -611.5 -756.8 -75 65016 11444 437840 123 30 -93.0 -620.8 -761.1 -75 58053 11466 437890 121 30 -91.0 -629.9 -766.8 -75 50136 11488 437940 118 30 -88.0 -638.7 -771.3 -75 41211 11509 437990 116 30 -86.0 -647.3 -776.7 -75 31216 11527 438040 111 30 -81.0 -655.4 -777.9 -75 20101 11544 438090 109 30 -79.0 -663.3 -782.2 -75 7792 11560 438140 107 30 -77.0 -671.0 -786.9 -75 1181 11575 438190 108 30 -78.0 -678.8 -795.6 -75 1190 11589 438240 102 30 -72.0 -686.0 -795.2 -75 1219 11600 438290 98 30 -68.0 -692.8 -795.6 -75 1240 11609 438340 96 30 -66.0 -699.4 -798.8 -75 1268 11615 438390 93 30 -63.0 -705.7 -800.8 -75 1283 11618 438440 91 30 -61.0 -711.8 -803.7 -75 1300 11619 438490 88 30 -58.0 -717.6 -805.2 -75 1321 11616 438540 88 30 -58.0 -723.4 -810.4 -75 1327 11610 438590 84 30 -54.0 -728.8 -810.6 -75 1339 11600 438640 82 30 -52.0 -734.0 -812.4 -75 1338 11586 438690 78 30 -48.0 -738.8 -811.6 -75 1349 11568 438740 76 30 -46.0 -743.4 -812.8 -75 1358 11545 438790 75 30 -45.0 -747.9 -815.6 -75 1352 11518 438840 73 30 -43.0 -752.2 -817.1 -75 1351 11486 438890 72 30 -42.0 -756.4 -819.6 -75 1352 11448 438940 69 30 -39.0 -760.3 -819.4 -75 1341 11406 438990 66 30 -36.0 -763.9 -818.5 -75 1340 11357 439043 65 30 -35.0 -767.4 -820.1 -75 1344 11303 439093 62 30 -32.0 -770.6 -819.2 -75 1336 11243 439143 60 30 -30.0 -773.6 -819.0 -75 1339 11176 439193 58 30 -28.0 -776.4 -818.8 -75 1329 11102 439243 53 30 -23.0 -778.7 -814.2 -75 1327 11022 439293 50 30 -20.0 -780.7 -811.3 -75 1327 10935 439343 47 30 -17.0 -782.4 -808.5 -75 1327 10840 439393 44 30 -14.0 -783.8 -805.4 -75 1332 10738 439443 42 30 -12.0 -785.0 -803.4 -75 1331 10627 439493 41 30 -11.0 -786.1 -802.8 -75 1320 10507 439543 38 30 -8.0 -786.9 -799.5 -75 1314 10379 439593 36 30 -6.0 -787.5 -796.9 -75 1306 10242 439643 33 30 -3.0 -787.8 -792.9 -75 1299 10098 439691 30 30 0.0 -787.8 -788.4 -75 1290 9947 MTFD: Exited with front dist = 29, anomaly code = WALL_OFFSET_DIST_AHEAD Front Rear Left Right 27 243.60 141.60 89.40 24 248.00 142.90 88.20 21 253.60 144.00 87.20 17 258.50 143.50 86.10 16 257.80 142.10 84.20 14 258.30 141.90 82.80 15 258.40 141.40 83.40 15 258.70 142.40 83.00 14 259.00 141.60 83.50 15 260.30 142.00 82.80 Second time through MTDFD with error = 15 MTFD: at start, tgt = 30cm, curr_dist = 14, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 440901 15 30 15.0 1.5 21.0 21 83583 12614 440951 16 30 14.0 2.9 24.1 24 84298 12259 441001 15 30 15.0 4.4 26.7 26 85373 11906 441051 16 30 14.0 5.8 27.0 27 86752 11558 441101 17 30 13.0 7.1 26.8 26 88414 11212 441151 16 30 14.0 8.5 29.3 29 90377 10870 441201 18 30 12.0 9.7 28.1 28 92563 10533 441251 17 30 13.0 11.0 30.3 30 95005 10200 441301 17 30 13.0 12.3 31.8 31 97662 9872 441351 17 30 13.0 13.6 33.1 33 100507 9548 441401 18 30 12.0 14.8 33.0 33 103499 9229 441451 19 30 11.0 15.9 32.6 32 106611 8915 441503 22 30 8.0 16.7 29.3 29 109781 8607 441551 21 30 9.0 17.6 30.9 30 113053 8304 441603 22 30 8.0 18.4 30.6 30 116361 8007 441651 22 30 8.0 19.2 31.2 31 119695 7716 441703 24 30 6.0 19.8 29.2 29 122990 7432 441753 25 30 5.0 20.3 28.0 28 126231 7155 441803 25 30 5.0 20.8 28.3 28 129403 6884 441853 25 30 5.0 21.3 28.8 28 132472 6620 441903 26 30 4.0 21.7 27.9 27 135388 6363 441953 26 30 4.0 22.1 28.1 28 138131 6114 442003 26 30 4.0 22.5 28.5 28 140665 5872 442053 28 30 2.0 22.7 26.1 26 142924 5638 442103 28 30 2.0 22.9 25.9 25 144899 5412 442153 29 30 1.0 23.0 24.7 24 146537 5193 MTFD: Exited with front dist = 29, anomaly code = WALL_OFFSET_DIST_AHEAD |
As can be seen in the above telemetry, the ‘Ival’ column shows that the integral term increases monotonically from -19.7 to -998.4, forcing the speed to its maximum value (75) for the entire run. This causes the robot to badly overshoot the target distance, which is why I decided to have the robot perform the ‘second time through’ action shown in the telemetry and video.
I added the following code to MoveToDesiredFrontDistCm() to zero out the ‘lastIval’ value if the error term * Kp > MOTOR_SPEED_QTR
1 2 3 4 5 |
//11/17/23 added to zero out lastIval anytime Kp*Err > max speed (MOTOR_SPEED_QTR here) if (abs(gl_FrontCm - offsetCm)*OffsetDistKp > MOTOR_SPEED_QTR) { lastIval = 0; } |
Here is the telemetry and the video from the run with the above change
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 |
MTFD: at start, tgt = 30cm, curr_dist = 221, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 151568 221 30 -191.0 -19.1 -267.4 -75 75698 12938 151618 220 30 -190.0 -19.0 -304.2 -75 73261 12763 151655 221 30 -191.0 -19.1 -305.4 -75 71147 12603 151705 219 30 -189.0 -18.9 -302.8 -75 69384 12457 151755 216 30 -186.0 -18.6 -298.2 -75 67972 12323 151805 216 30 -186.0 -18.6 -297.6 -75 66867 12197 151855 212 30 -182.0 -18.2 -292.0 -75 66104 12082 151905 210 30 -180.0 -18.0 -288.4 -75 65646 11978 151955 212 30 -182.0 -18.2 -290.8 -75 65431 11884 152005 208 30 -178.0 -17.8 -285.6 -75 65517 11798 152057 204 30 -174.0 -17.4 -279.2 -75 65888 11720 152108 206 30 -176.0 -17.6 -281.2 -75 66451 11649 152158 201 30 -171.0 -17.1 -274.6 -75 67277 11587 152208 195 30 -165.0 -16.5 -265.2 -75 68358 11524 152257 194 30 -164.0 -16.4 -262.6 -75 69609 11471 152307 194 30 -164.0 -16.4 -262.4 -75 70996 11423 152359 191 30 -161.0 -16.1 -258.2 -75 72533 11382 152410 192 30 -162.0 -16.2 -259.0 -75 74147 11347 152460 187 30 -157.0 -15.7 -252.2 -75 75886 11318 152510 184 30 -154.0 -15.4 -247.0 -75 77698 11294 152560 184 30 -154.0 -15.4 -246.4 -75 79518 11273 152609 180 30 -150.0 -15.0 -240.8 -75 81367 11257 152659 179 30 -149.0 -14.9 -238.6 -75 83177 11246 152709 177 30 -147.0 -14.7 -235.6 -75 84928 11240 152760 172 30 -142.0 -14.2 -228.2 -75 86624 11236 152809 170 30 -140.0 -14.0 -224.4 -75 88194 11238 152859 170 30 -140.0 -14.0 -224.0 -75 89580 11242 152909 166 30 -136.0 -13.6 -218.4 -75 90793 11245 152960 165 30 -135.0 -13.5 -216.2 -75 91759 11253 153009 162 30 -132.0 -13.2 -211.8 -75 92464 11258 153059 162 30 -132.0 -13.2 -211.2 -75 92835 11266 153109 159 30 -129.0 -12.9 -207.0 -75 92863 11277 153159 154 30 -124.0 -12.4 -199.4 -75 92524 11291 153209 150 30 -120.0 -12.0 -192.8 -75 91764 11307 153259 147 30 -117.0 -11.7 -187.8 -75 90525 11326 153309 144 30 -114.0 -11.4 -183.0 -75 88761 11345 153359 141 30 -111.0 -11.1 -178.2 -75 86423 11365 153409 139 30 -109.0 -10.9 -174.8 -75 83455 11386 153459 137 30 -107.0 -10.7 -171.6 -75 79807 11408 153509 134 30 -104.0 -10.4 -167.0 -75 75435 11430 153559 134 30 -104.0 -10.4 -166.4 -75 70267 11454 153611 131 30 -101.0 -10.1 -162.2 -75 64270 11476 153661 126 30 -96.0 -9.6 -154.6 -75 57400 11500 153711 124 30 -94.0 -9.4 -150.8 -75 49582 11523 153761 123 30 -93.0 -9.3 -149.0 -75 40756 11543 153811 118 30 -88.0 -8.8 -141.8 -75 30879 11563 153861 116 30 -86.0 -8.6 -138.0 -75 19879 11583 153911 113 30 -83.0 -8.3 -133.4 -75 7698 11601 153961 110 30 -80.0 -8.0 -128.6 -75 1162 11618 154011 111 30 -81.0 -8.1 -129.4 -75 1186 11633 154061 106 30 -76.0 -7.6 -122.6 -75 1211 11646 154111 105 30 -75.0 -7.5 -120.2 -75 1229 11657 154161 102 30 -72.0 -7.2 -115.8 -75 1242 11665 154211 99 30 -69.0 -6.9 -111.0 -75 1256 11670 154261 94 30 -64.0 -6.4 -103.4 -75 1278 11672 154311 94 30 -64.0 -6.4 -102.4 -75 1287 11670 154361 91 30 -61.0 -6.1 -98.2 -75 1302 11665 154411 89 30 -59.0 -5.9 -94.8 -75 1315 11656 154461 86 30 -56.0 -5.6 -90.2 -75 1318 11643 154511 83 30 -53.0 -5.3 -85.4 -75 1326 11625 154561 82 30 -52.0 -5.2 -83.4 -75 1335 11603 154611 79 30 -49.0 -4.9 -79.0 -75 1333 11575 154662 78 30 -48.0 -9.7 -81.9 -75 1334 11542 154712 76 30 -46.0 -14.3 -83.7 -75 1342 11503 154761 74 30 -44.0 -18.7 -85.1 -75 1347 11458 154812 73 30 -43.0 -23.0 -87.7 -75 1342 11406 154862 70 30 -40.0 -27.0 -87.6 -75 1339 11349 154912 68 30 -38.0 -30.8 -88.2 -75 1328 11285 154961 64 30 -34.0 -34.2 -86.0 -75 1326 11214 155011 62 30 -32.0 -37.4 -85.8 -75 1324 11136 155062 58 30 -28.0 -40.2 -83.0 -75 1320 11051 155111 57 30 -27.0 -42.9 -83.6 -75 1316 10957 155162 54 30 -24.0 -45.3 -81.9 -75 1310 10856 155212 51 30 -21.0 -47.4 -79.5 -75 1303 10745 155262 47 30 -17.0 -49.1 -75.4 -75 1307 10626 155312 44 30 -14.0 -50.5 -72.1 -72 1311 10497 155362 41 30 -11.0 -51.6 -68.7 -68 1309 10359 155412 39 30 -9.0 -52.5 -66.4 -66 1309 10211 155462 38 30 -8.0 -53.3 -65.5 -65 1302 10057 155512 36 30 -6.0 -53.9 -63.3 -63 1294 9895 155562 32 30 -2.0 -54.1 -57.9 -57 1284 9723 155612 32 30 -2.0 -54.3 -57.3 -57 1268 9541 155660 29 30 1.0 -54.2 -53.3 -53 1260 9348 MTFD: Exited with front dist = 29, anomaly code = WALL_OFFSET_DIST_AHEAD Front Rear Left Right 29 243.90 138.00 89.20 24 251.70 144.50 87.30 21 253.20 144.40 85.30 19 254.80 143.40 81.70 19 256.50 142.40 80.60 19 254.90 142.00 79.90 17 254.70 142.70 79.90 18 254.70 141.30 79.80 18 254.90 142.20 79.70 17 255.40 142.20 80.10 Second time through MTDFD with error = 13 MTFD: at start, tgt = 30cm, curr_dist = 17, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 156872 18 30 12.0 1.2 16.8 16 83420 12604 156922 19 30 11.0 2.3 19.0 19 84078 12245 156972 17 30 13.0 3.6 22.7 22 85116 11889 157022 18 30 12.0 4.8 23.0 23 86458 11536 157072 17 30 13.0 6.1 25.4 25 88121 11187 157122 17 30 13.0 7.4 26.9 26 90067 10844 157172 18 30 12.0 8.6 26.8 26 92254 10506 157222 19 30 11.0 9.7 26.4 26 94662 10171 157272 18 30 12.0 10.9 28.7 28 97302 9842 157322 19 30 11.0 12.0 28.7 28 100114 9518 157372 19 30 11.0 13.1 29.6 29 103092 9200 157422 20 30 10.0 14.1 29.3 29 106190 8887 157472 20 30 10.0 15.1 30.1 30 109400 8579 157522 21 30 9.0 16.0 29.7 29 112676 8278 157572 23 30 7.0 16.7 27.6 27 115973 7982 157622 22 30 8.0 17.5 29.3 29 119313 7693 157673 22 30 8.0 18.3 30.3 30 122646 7411 157722 24 30 6.0 18.9 28.3 28 125909 7135 157772 24 30 6.0 19.5 28.5 28 129103 6866 157822 26 30 4.0 19.9 26.3 26 132162 6604 157872 25 30 5.0 20.4 27.7 27 135100 6349 157922 26 30 4.0 20.8 27.0 27 137849 6102 157973 25 30 5.0 21.3 28.6 28 140404 5862 158022 25 30 5.0 21.8 29.3 29 142712 5630 158072 27 30 3.0 22.1 27.0 27 144706 5405 158122 26 30 4.0 22.5 28.3 28 146390 5188 158173 27 30 3.0 22.8 27.5 27 147696 4979 158222 29 30 1.0 22.9 24.8 24 148571 4778 MTFD: Exited with front dist = 28, anomaly code = WALL_OFFSET_DIST_AHEAD |
As can be seen from the above, this change did not affect the robot’s behavior significantly – it still badly overshot the target distance and the ‘second try’ was still required to bring the robot back to nearer the target. However, from inspection of the ‘Ival’ values it is clear that the added code is doing its job of zeroing out the ‘lastIval’ term when [error_term]*Kp > Max speed. Here’s an Excel plot showing the ‘Ival’ and ‘output’ terms from both the above runs.
In the above plot, the ‘after Ival term stays at a very low (negative) value for almost the entire run, due to the new clamping code. Consequently the ‘after’ output term decreases linearly with the decreasing error term until the resultant ‘after’ speed command comes off the -75 ‘stop’ as shown by the gray line in the above plot.
Unfortunately, the speed reduction from ‘lastIval’ clamping isn’t enough to prevent the robot from overshooting almost as much as it did before. This indicates (at least to me) that at least the Kp value is way too high. Just as a thought, the Kp value should be just high enough so that if the distance error is, say, 100cm, then the output would just clamp at MOTOR_SPEED_LOW –> 75. So, 100*Kp = 75 –> Kp = 0.75, or about half its current value.
It turned out that I not only needed to cut the Kp value in half, but the Ki value as well, so now PID = (0.75, 0.05, 0.2). Here’s the telemetry and video from a run using these values:
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 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 |
Moving Front to 30 cm, with PID = (0.75,0.05,0.20) Enter any key to start . . . . . . Before MTDFD, FVar = 74798.82 MTFD: at start, tgt = 30cm, curr_dist = 204, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 632936 204 30 -174.0 -8.7 -104.4 -75 76171 12819 632986 207 30 -177.0 -8.9 -141.0 -75 73892 12584 633036 205 30 -175.0 -8.8 -140.4 -75 71975 12362 633086 204 30 -174.0 -8.7 -139.4 -75 70399 12151 633136 202 30 -172.0 -8.6 -138.0 -75 69165 11952 633186 201 30 -171.0 -8.6 -137.0 -75 68250 11762 633236 200 30 -170.0 -8.5 -136.2 -75 67641 11583 633286 197 30 -167.0 -8.4 -134.2 -75 67350 11412 633336 199 30 -169.0 -8.4 -134.8 -75 67298 11250 633385 193 30 -163.0 -8.2 -131.6 -75 67572 11099 633436 193 30 -163.0 -8.2 -130.4 -75 68079 10955 633485 188 30 -158.0 -7.9 -127.4 -75 68864 10820 633535 188 30 -158.0 -7.9 -126.4 -75 69843 10690 633585 184 30 -154.0 -7.7 -124.0 -75 71048 10568 633635 181 30 -151.0 -7.6 -121.4 -75 72444 10451 633685 179 30 -149.0 -7.5 -119.6 -75 73996 10336 633735 175 30 -145.0 -7.2 -116.8 -75 75704 10227 633786 176 30 -146.0 -7.3 -116.6 -75 77479 10126 633836 172 30 -142.0 -7.1 -114.4 -75 79359 10031 633886 172 30 -142.0 -7.1 -113.6 -75 81266 9945 633936 169 30 -139.0 -7.0 -111.8 -75 83208 9864 633986 165 30 -135.0 -6.8 -108.8 -75 85168 9790 634035 163 30 -133.0 -6.7 -106.8 -75 87089 9722 634088 163 30 -133.0 -6.7 -106.4 -75 88916 9660 634138 161 30 -131.0 -6.6 -105.2 -75 90638 9602 634188 157 30 -127.0 -6.3 -102.4 -75 92246 9551 634238 152 30 -122.0 -6.1 -98.6 -75 93714 9505 634288 153 30 -123.0 -6.2 -98.2 -75 94934 9463 634338 148 30 -118.0 -5.9 -95.4 -75 95937 9424 634388 149 30 -119.0 -6.0 -95.0 -75 96617 9386 634438 143 30 -113.0 -5.7 -91.6 -75 97009 9355 634488 142 30 -112.0 -5.6 -89.8 -75 97016 9327 634538 140 30 -110.0 -5.5 -88.4 -75 96606 9304 634588 136 30 -106.0 -5.3 -85.6 -75 95755 9285 634637 134 30 -104.0 -5.2 -83.6 -75 94396 9269 634688 131 30 -101.0 -5.1 -81.4 -75 92492 9256 634738 129 30 -99.0 -5.0 -79.6 -75 89985 9245 634788 127 30 -97.0 -4.8 -78.0 -75 86826 9238 634837 125 30 -95.0 -4.8 -76.4 -75 82966 9233 634887 122 30 -92.0 -4.6 -74.2 -74 78360 9231 634937 120 30 -90.0 -4.5 -72.4 -72 72947 9229 634987 116 30 -86.0 -4.3 -69.6 -69 66687 9229 635037 114 30 -84.0 -4.2 -67.6 -67 59509 9231 635087 112 30 -82.0 -4.1 -66.0 -66 51359 9233 635137 109 30 -79.0 -4.0 -63.8 -63 42183 9238 635187 106 30 -76.0 -3.8 -61.4 -61 31921 9244 635237 105 30 -75.0 -3.8 -60.2 -60 20506 9252 635287 100 30 -70.0 -3.5 -57.0 -57 7888 9260 635337 100 30 -70.0 -3.5 -56.0 -56 1117 9268 635387 99 30 -69.0 -3.5 -55.4 -55 1139 9274 635437 97 30 -67.0 -3.4 -54.0 -54 1159 9280 635487 93 30 -63.0 -3.2 -51.2 -51 1172 9285 635537 93 30 -63.0 -3.2 -50.4 -50 1180 9290 635587 89 30 -59.0 -3.0 -48.0 -48 1189 9293 635637 88 30 -58.0 -2.9 -46.6 -46 1194 9296 635687 87 30 -57.0 -2.9 -45.8 -45 1193 9297 635737 84 30 -54.0 -2.7 -43.8 -43 1191 9297 635787 83 30 -53.0 -2.7 -42.6 -42 1187 9294 635837 81 30 -51.0 -2.5 -41.2 -41 1172 9289 635887 80 30 -50.0 -2.5 -40.2 -40 1164 9282 635940 77 30 -47.0 -4.8 -40.7 -40 1151 9271 635990 76 30 -46.0 -7.2 -41.9 -41 1142 9257 636040 75 30 -45.0 -9.4 -43.4 -43 1126 9239 636090 74 30 -44.0 -11.6 -44.8 -44 1110 9217 636140 73 30 -43.0 -13.8 -46.2 -46 1095 9191 636188 70 30 -40.0 -15.8 -46.3 -46 1080 9160 636238 67 30 -37.0 -17.6 -45.9 -45 1072 9125 636288 66 30 -36.0 -19.4 -46.6 -46 1053 9083 636338 67 30 -37.0 -21.2 -48.8 -48 1033 9036 636390 65 30 -35.0 -23.0 -49.7 -49 1008 8984 636440 65 30 -35.0 -24.8 -51.0 -51 980 8924 636490 63 30 -33.0 -26.4 -51.6 -51 958 8859 636540 61 30 -31.0 -27.9 -51.6 -51 935 8787 636590 58 30 -28.0 -29.3 -50.9 -50 909 8707 636640 57 30 -27.0 -30.7 -51.1 -51 880 8621 636690 53 30 -23.0 -31.8 -49.9 -49 860 8524 636740 51 30 -21.0 -32.9 -49.0 -49 846 8421 636790 49 30 -19.0 -33.8 -48.5 -48 825 8307 636840 48 30 -18.0 -34.8 -48.5 -48 809 8184 636890 46 30 -16.0 -35.5 -48.0 -47 786 8052 636940 45 30 -15.0 -36.3 -47.8 -47 771 7910 636990 43 30 -13.0 -37.0 -47.1 -47 753 7758 637040 41 30 -11.0 -37.5 -46.2 -46 735 7596 637090 40 30 -10.0 -38.0 -45.7 -45 720 7425 637140 37 30 -7.0 -38.3 -44.2 -44 706 7244 637190 34 30 -4.0 -38.5 -42.1 -42 697 7052 637240 34 30 -4.0 -38.8 -41.8 -41 685 6848 637290 33 30 -3.0 -38.9 -41.4 -41 671 6632 637338 30 30 0.0 -38.9 -39.5 -39 660 6403 MTFD: Exited with front dist = 30, anomaly code = WALL_OFFSET_DIST_AHEAD Front Rear Left Right 30 256.60 153.50 74.30 27 260.80 154.20 74.60 24 263.50 155.60 74.90 25 265.70 156.10 73.50 25 265.60 156.90 74.30 24 265.90 157.50 73.70 25 266.70 156.20 75.80 23 264.50 156.70 73.90 25 265.50 157.50 75.00 24 266.00 157.20 73.20 Second time through MTDFD with error = 6 MTFD: at start, tgt = 30cm, curr_dist = 25, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 638548 23 30 7.0 0.3 4.2 4 83116 12624 638598 25 30 5.0 0.6 4.8 4 83662 12274 638648 24 30 6.0 0.9 5.2 5 84568 11928 638698 24 30 6.0 1.2 5.7 5 85799 11585 638748 24 30 6.0 1.5 6.0 6 87333 11245 638798 25 30 5.0 1.8 5.7 5 89133 10908 638851 26 30 4.0 2.0 5.1 5 91178 10575 638901 26 30 4.0 2.2 5.2 5 93464 10245 638951 26 30 4.0 2.4 5.4 5 95967 9920 639001 25 30 5.0 2.6 6.2 6 98682 9598 639051 25 30 5.0 2.9 6.6 6 101566 9282 639101 24 30 6.0 3.2 7.5 7 104609 8970 639151 25 30 5.0 3.4 7.3 7 107749 8665 639201 25 30 5.0 3.7 7.4 7 110977 8364 639251 23 30 7.0 4.0 8.9 8 114296 8069 639301 24 30 6.0 4.3 9.0 9 117627 7779 639351 24 30 6.0 4.6 9.1 9 120954 7496 639401 25 30 5.0 4.9 8.8 8 124229 7217 639451 25 30 5.0 5.1 8.9 8 127438 6944 639501 25 30 5.0 5.4 9.1 9 130545 6679 639551 25 30 5.0 5.6 9.4 9 133516 6420 639601 24 30 6.0 5.9 10.2 10 136331 6167 639651 26 30 4.0 6.1 9.5 9 138908 5921 639701 26 30 4.0 6.3 9.3 9 141239 5681 639751 25 30 5.0 6.6 10.1 10 143301 5448 639801 24 30 6.0 6.9 11.2 11 145053 5221 639851 23 30 7.0 7.2 12.3 12 146453 5002 639901 25 30 5.0 7.5 11.6 11 147419 4789 639951 26 30 4.0 7.7 10.8 10 147926 4584 640001 26 30 4.0 7.8 10.9 10 147942 4385 640051 25 30 5.0 8.1 11.7 11 147434 4194 640101 24 30 6.0 8.4 12.7 12 146356 4010 640151 25 30 5.0 8.7 12.6 12 144639 3833 640201 26 30 4.0 8.9 12.1 12 142238 3663 640251 26 30 4.0 9.1 12.1 12 139116 3500 640301 25 30 5.0 9.3 12.9 12 135231 3345 640351 25 30 5.0 9.6 13.3 13 130524 3196 640401 25 30 5.0 9.8 13.6 13 124945 3055 640451 25 30 5.0 10.1 13.8 13 118440 2920 640501 25 30 5.0 10.3 14.1 14 110958 2792 640551 26 30 4.0 10.5 13.7 13 102438 2671 640601 24 30 6.0 10.8 14.9 14 92842 2557 640651 25 30 5.0 11.1 15.0 15 82099 2449 640701 25 30 5.0 11.3 15.1 15 70156 2349 640751 25 30 5.0 11.6 15.3 15 56956 2254 640801 25 30 5.0 11.8 15.6 15 42441 2166 640851 25 30 5.0 12.1 15.8 15 26549 2084 640901 25 30 5.0 12.3 16.0 16 9222 2008 640951 24 30 6.0 12.6 16.9 16 1 1939 641001 24 30 6.0 12.9 17.4 17 1 1875 641051 26 30 4.0 13.1 16.5 16 1 1816 641101 25 30 5.0 13.4 16.9 16 1 1763 641151 25 30 5.0 13.6 17.4 17 1 1716 641203 24 30 6.0 13.9 18.2 18 1 1673 641253 25 30 5.0 14.2 18.1 18 1 1636 641303 24 30 6.0 14.5 18.8 18 1 1604 641353 24 30 6.0 14.8 19.2 19 1 1575 641403 24 30 6.0 15.1 19.6 19 1 1551 641453 24 30 6.0 15.4 19.9 19 1 1531 641503 25 30 5.0 15.6 19.6 19 1 1514 641553 25 30 5.0 15.9 19.6 19 1 1501 641603 26 30 4.0 16.1 19.3 19 1 1491 641653 26 30 4.0 16.3 19.3 19 1 1484 641703 26 30 4.0 16.5 19.5 19 1 1478 641753 25 30 5.0 16.7 20.3 20 1 1476 641803 25 30 5.0 17.0 20.7 20 1 1476 641853 25 30 5.0 17.2 21.0 20 1 1477 641903 26 30 4.0 17.4 20.6 20 1 1480 641953 26 30 4.0 17.6 20.6 20 1 1484 642003 26 30 4.0 17.8 20.8 20 1 1489 642053 26 30 4.0 18.0 21.0 21 1 1494 642103 27 30 3.0 18.2 20.6 20 1 1499 642153 27 30 3.0 18.3 20.6 20 1 1503 642203 28 30 2.0 18.4 20.1 20 1 1507 642253 29 30 1.0 18.5 19.4 19 1 1508 MTFD: Exited with front dist = 28, anomaly code = WALL_OFFSET_DIST_AHEAD |
I wasn’t able to eliminate the ‘second try’ requirement, although I was able to reduce the overshoot to about half its previous value.
One possible fly in the ointment is the rapid drop of the front variance value during the second run through MoveToDesiredFrontDistCm(); this *should* have triggered an exit from the function with anomaly code = ANOMALY_STUCK_AHEAD, but didn’t – and I don’t know why.
Too tired to go down this rabbit hole tonight – try again tomorrow while waiting for Space X to make history with its second StarShip test flight!
18 November 2023 Update:
Wow! Wow! Wow! I had the pleasure of watching SpaceX’s literally historic second Starship test launch this morning, and I’m still psyched! I have now had the pleasure of watching both a Saturn V launch as part of the Apollo moon landing program and now the Starship and booster launch as part of (I hope) the Mars landing program.
OK, back to robots. Last night while drifting off to sleep, it occurred to me that the ANOMALY_STUCK_AHEAD alert is conditioned on the current motor configuration as well as the front variance value. Here is the actual code that determines this:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
bool IsStuckAhead() { //Purpose: Determine if robot is stuck in forward direction //Inputs: // gl_Frontvar = uint16_t denoting current front variance value // gl_FrontCm = uint16_t denoting current front distance (cm) // motor status //Notes: // 05/05/23 added check for both motors stopped //gl_pSerPort->printf("IsStuckAhead() with FrontD = %d, FrontV = %2.0f, AreBothMotorsForward() = %d, AreBothMotorsStopped() = %d\n", // gl_FrontCm, gl_Frontvar, AreBothMotorsForward(), AreBothMotorsStopped()); return (gl_Frontvar < STUCK_FRONT_VARIANCE_THRESHOLD) && (gl_FrontCm < MAX_FRONT_DISTANCE_CM) && (AreBothMotorsForward() || AreBothMotorsStopped()); } |
So in this case, the motors were running in reverse, so neither of the motor conditions were TRUE, and therefore IsStuckAhead() returned FALSE. Mystery solved!
At this point I think I’ve done as much as I can with the FrontBackMotionTest program. I will modify the other ‘MoveTo’ functions to clamp the Ival term as described above. However, it is also clear that the ‘second try’ part of the ‘MoveTo’ functions cannot be eliminated without having to accept significant target distance overshoot, and that the original PID values (1.5, 0.1, 0.2) actually work better than (0.75, 0.05, 0.2), as the lower PI values cause the second try operation to take significantly longer. Here’s an Excel plot comparing just the ‘second try’ results for both:
In the above plot, the ‘2’ results are for (0.75, 0.05, 0.2). As can be seen, the ‘2’ configuration takes over twice as long to complete as the ‘normal’ (1.5, 0.1, 0.2) run, and the issue with the front variance value going to zero doesn’t exist for the ‘normal’ run.
This makes it clear that while clamping the Ival does help significantly, the change in PID values does not.
18 November 2023 8:43PM EST Update:
While cleaning up the code, I noticed I had missed one spot where the global OffsetDistKp value should have been replaced by ‘Kp’, the local value, and unfortunately it was in the section that determined whether or not the Ival value would be clamped, as shown below:
1 2 3 4 5 |
//if (abs(gl_FrontCm - offsetCm)*OffsetDistKp > MOTOR_SPEED_QTR) if (abs(gl_FrontCm - offsetCm) * Kp > MOTOR_SPEED_QTR) { lastIval = 0; } |
This had the effect of skewing the point at which the Ival stopped being clamped, so the previous results are a bit suspect. I ran a few more tests, and discovered the function actually performed better with a higher Kp value than the original 1.5. The reason for this is that a higher Kp value moves the point at which Ival can start accumulating later in the run (closer to the target distance thus less error), and so more quickly reduces motor speed. In addition, the Ival clamping action has no effect on the second pass through the function as the motor speed values never approach the max speed, so this part operates as designed as well. Here’s a run showing the result of using a PID of (2.0, 0.1, 0.2):
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 |
Moving Front to 30 cm, with PID = (2.00,0.10,0.20) Enter any key to start . . . . Before MTDFD, FVar = 77648.85 MTFD: at start, tgt = 30cm, curr_dist = 201, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 1405356 204 30 -174.0 -17.4 -330.6 -75 76213 12892 1405407 201 30 -171.0 -17.1 -359.7 -75 74008 12693 1405459 202 30 -172.0 -17.2 -361.0 -75 72128 12508 1405509 201 30 -171.0 -17.1 -359.3 -75 70590 12338 1405559 201 30 -171.0 -17.1 -359.1 -75 69370 12179 1405609 199 30 -169.0 -16.9 -355.3 -75 68481 12031 1405659 197 30 -167.0 -16.7 -351.1 -75 67910 11892 1405709 193 30 -163.0 -16.3 -343.1 -75 67671 11763 1405759 193 30 -163.0 -16.3 -342.3 -75 67696 11643 1405809 190 30 -160.0 -16.0 -336.6 -75 68009 11531 1405859 189 30 -159.0 -15.9 -334.1 -75 68566 11426 1405909 187 30 -157.0 -15.7 -330.1 -75 69362 11329 1405959 182 30 -152.0 -15.2 -320.2 -75 70416 11239 1406008 180 30 -150.0 -15.0 -315.4 -75 71670 11156 1406059 181 30 -151.0 -15.1 -316.9 -75 73061 11080 1406109 178 30 -148.0 -14.8 -311.4 -75 74619 11005 1406159 177 30 -147.0 -14.7 -308.9 -75 76294 10935 1406209 174 30 -144.0 -14.4 -303.0 -75 78087 10872 1406258 172 30 -142.0 -14.2 -298.6 -75 79958 10815 1406308 169 30 -139.0 -13.9 -292.5 -75 81893 10764 1406358 169 30 -139.0 -13.9 -291.9 -75 83824 10719 1406408 167 30 -137.0 -13.7 -288.1 -75 85746 10680 1406458 161 30 -131.0 -13.1 -276.3 -75 87680 10643 1406508 158 30 -128.0 -12.8 -269.4 -75 89552 10612 1406558 159 30 -129.0 -12.9 -270.7 -75 91282 10586 1406608 157 30 -127.0 -12.7 -267.1 -75 92871 10564 1406658 151 30 -121.0 -12.1 -255.3 -75 94331 10546 1406708 152 30 -122.0 -12.2 -256.0 -75 95542 10527 1406758 149 30 -119.0 -11.9 -250.5 -75 96511 10512 1406808 148 30 -118.0 -11.8 -248.0 -75 97179 10500 1406858 145 30 -115.0 -11.5 -242.1 -75 97525 10492 1406908 141 30 -111.0 -11.1 -233.9 -75 97517 10487 1406961 138 30 -108.0 -10.8 -227.4 -75 97101 10484 1407011 135 30 -105.0 -10.5 -221.1 -75 96231 10482 1407061 133 30 -103.0 -10.3 -216.7 -75 94851 10484 1407111 130 30 -100.0 -10.0 -210.6 -75 92924 10486 1407160 128 30 -98.0 -9.8 -206.2 -75 90392 10490 1407210 125 30 -95.0 -9.5 -200.1 -75 87214 10496 1407260 124 30 -94.0 -9.4 -197.6 -75 83323 10504 1407310 121 30 -91.0 -9.1 -191.7 -75 78685 10513 1407360 118 30 -88.0 -8.8 -185.4 -75 73245 10524 1407410 116 30 -86.0 -8.6 -181.0 -75 66941 10532 1407460 116 30 -86.0 -8.6 -180.6 -75 59708 10541 1407510 110 30 -80.0 -8.0 -169.2 -75 51523 10550 1407560 110 30 -80.0 -8.0 -168.0 -75 42295 10559 1407610 107 30 -77.0 -7.7 -162.3 -75 31981 10568 1407660 105 30 -75.0 -7.5 -157.9 -75 20517 10577 1407710 103 30 -73.0 -7.3 -153.7 -75 7841 10584 1407760 100 30 -70.0 -7.0 -147.6 -75 1046 10592 1407810 97 30 -67.0 -6.7 -141.3 -75 1071 10599 1407860 95 30 -65.0 -6.5 -136.9 -75 1090 10605 1407910 94 30 -64.0 -6.4 -134.6 -75 1107 10610 1407960 92 30 -62.0 -6.2 -130.6 -75 1118 10613 1408010 91 30 -61.0 -6.1 -128.3 -75 1123 10615 1408060 89 30 -59.0 -5.9 -124.3 -75 1124 10614 1408110 86 30 -56.0 -5.6 -118.2 -75 1125 10612 1408160 83 30 -53.0 -5.3 -111.9 -75 1127 10608 1408210 82 30 -52.0 -5.2 -109.4 -75 1130 10600 1408260 78 30 -48.0 -4.8 -101.6 -75 1133 10590 1408310 75 30 -45.0 -4.5 -95.1 -75 1138 10577 1408360 73 30 -43.0 -4.3 -90.7 -75 1140 10561 1408411 71 30 -41.0 -4.1 -86.5 -75 1140 10541 1408460 71 30 -41.0 -4.1 -86.1 -75 1141 10517 1408511 68 30 -38.0 -3.8 -80.4 -75 1144 10489 1408561 66 30 -36.0 -3.6 -76.0 -75 1138 10456 1408611 65 30 -35.0 -7.1 -77.3 -75 1132 10419 1408660 61 30 -31.0 -10.2 -73.0 -73 1126 10377 1408710 59 30 -29.0 -13.1 -71.5 -71 1121 10329 1408761 56 30 -26.0 -15.7 -68.3 -68 1116 10277 1408811 53 30 -23.0 -18.0 -64.6 -64 1115 10218 1408861 51 30 -21.0 -20.1 -62.5 -62 1108 10153 1408911 48 30 -18.0 -21.9 -58.5 -58 1101 10083 1408961 46 30 -16.0 -23.5 -55.9 -55 1102 10005 1409011 44 30 -14.0 -24.9 -53.3 -53 1104 9920 1409063 41 30 -11.0 -26.0 -48.6 -48 1100 9828 1409111 39 30 -9.0 -26.9 -45.3 -45 1094 9728 1409163 38 30 -8.0 -27.7 -43.9 -43 1094 9621 1409211 33 30 -3.0 -28.0 -35.0 -35 1092 9506 1409261 34 30 -4.0 -28.4 -36.2 -36 1085 9384 1409313 32 30 -2.0 -28.6 -33.0 -33 1073 9251 1409363 31 30 -1.0 -28.7 -30.9 -30 1060 9108 MTFD: Exited with front dist = 31, anomaly code = NONE Front Rear Left Right 30 248.70 140.50 92.10 27 252.80 141.80 90.30 26 253.80 141.90 90.00 25 254.70 142.60 89.60 25 256.00 141.60 89.40 25 255.60 142.10 89.30 25 255.00 143.10 89.70 24 255.00 141.90 89.00 23 256.10 142.00 89.70 24 253.50 142.60 89.60 Second time through MTDFD with error = 6 MTFD: at start, tgt = 30cm, curr_dist = 25, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 1410570 25 30 5.0 0.5 9.5 9 83060 12604 1410620 25 30 5.0 1.0 11.0 11 83605 12243 1410670 24 30 6.0 1.6 13.4 13 84512 11888 1410720 24 30 6.0 2.2 14.2 14 85742 11536 1410770 24 30 6.0 2.8 14.8 14 87277 11188 1410823 24 30 6.0 3.4 15.4 15 89096 10846 1410873 24 30 6.0 4.0 16.0 16 91178 10506 1410923 24 30 6.0 4.6 16.6 16 93500 10172 1410973 23 30 7.0 5.3 19.1 19 96058 9843 1411023 24 30 6.0 5.9 18.1 18 98790 9519 1411073 25 30 5.0 6.4 16.6 16 101673 9201 1411123 24 30 6.0 7.0 18.8 18 104715 8888 1411173 24 30 6.0 7.6 19.6 19 107871 8580 1411223 25 30 5.0 8.1 18.3 18 111097 8280 1411273 25 30 5.0 8.6 18.6 18 114381 7985 1411323 24 30 6.0 9.2 21.0 21 117710 7697 1411373 26 30 4.0 9.6 18.0 17 121003 7415 1411423 25 30 5.0 10.1 19.9 19 124278 7140 1411473 25 30 5.0 10.6 20.6 20 127485 6871 1411523 25 30 5.0 11.1 21.1 21 130592 6610 1411573 25 30 5.0 11.6 21.6 21 133562 6355 1411623 26 30 4.0 12.0 20.2 20 136346 6107 1411673 25 30 5.0 12.5 22.3 22 138937 5866 1411723 24 30 6.0 13.1 24.9 24 141296 5633 1411773 26 30 4.0 13.5 21.9 21 143343 5407 1411823 27 30 3.0 13.8 20.0 20 145053 5188 1411873 27 30 3.0 14.1 20.1 20 146401 4976 1411923 27 30 3.0 14.4 20.4 20 147344 4772 1411973 27 30 3.0 14.7 20.7 20 147841 4575 1412023 27 30 3.0 15.0 21.0 21 147849 4386 1412073 27 30 3.0 15.3 21.3 21 147323 4204 1412123 28 30 2.0 15.5 19.7 19 146208 4030 1412173 27 30 3.0 15.8 21.6 21 144478 3863 1412223 28 30 2.0 16.0 20.2 20 142067 3703 1412273 30 30 0.0 16.0 16.4 16 138919 3551 MTFD: Exited with front dist = 29, anomaly code = WALL_OFFSET_DIST_AHEAD |
As the data shows, the overshoot is only 6cm (as opposed to the 18cm overshoot without the Ival clamp), and the recovery pass completes in about 2 sec instead of over 5. Here are Excel plots showing the primary and secondary passes.
So, it looks like I want to change the global OffsetKp value from 1.5 to 2.0, leaving Ki & Kd unchanged.
19 November 2023 Update:
Looking back at the results from the above tests, I realized that the second call to MoveToDesiredFrontDistCm(Offset, Kp, Ki, Kd) (the ‘second time though’ step) isn’t actually in the MoveToDesiredFrontDistCm() function – it is part of the test harness. So, I think I need to incorporate the second pass directly into the function so it will get performed everywhere MoveToDesiredFrontDistCm() is called. I wonder if I can do this recursively, by calling MoveToDesiredFrontDistCm() from inside MoveToDesiredFrontDistCm()?
So, I moved the ‘Second Time Through’ code from the test harness into MoveToDesiredFrontDistCm(), 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 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 |
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. // 10/23/23 rev to include anomaly handling for STUCK_AHEAD/BEHIND anomaly types // 11/16/23 revised to include Kp,Ki,Kd parameters as overload to original 1-parameter fcn // to allow test harness to call repeatedly with different PID sets 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, OffsetDistKp, OffsetDistKi, OffsetDistKd); //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.0f/%2.0f\n", offsetCm, GetFrontDistCm(), gl_Frontvar, gl_Rearvar); //gl_pSerPort->printf("Msec\tFdist\tTgtD\terr\tIval\tOut\tSpeed\tFVar\tRVar\n"); UpdateAllEnvironmentParameters(); MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate gl_LastAnomalyCode = ANOMALY_NONE; gl_pSerPort->printf("Msec\tFrontD\tTgtD\tErr\tIval\tOutput\tspeed\tFrontVar\tRearVar\n"); //curdiff = lastdiff = gl_FrontCm - offsetCm; //11/12/23 this can be positive or neg //11/17/23 rev to zero out lastIval anytime Kp*Err > max speed (MOTOR_SPEED_QTR here) while (abs(gl_FrontCm - offsetCm) > 1 && gl_LastAnomalyCode != ANOMALY_STUCK_AHEAD && gl_LastAnomalyCode != ANOMALY_STUCK_BEHIND) { CheckForUserInput(); if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE) { MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE; UpdateAllEnvironmentParameters(); //10/01/23 this can change anomaly code ////11/12/23 update last/curdiff values for sign change detection //lastdiff = curdiff; //curdiff = gl_FrontCm - offsetCm; //11/12/23 now only the outer 'while()' loop can detect/handle anomalies ////10/23/23 only handle these two anomalies //if (gl_LastAnomalyCode == ANOMALY_STUCK_AHEAD || gl_LastAnomalyCode == ANOMALY_STUCK_BEHIND) //{ // HandleAnomalousConditions(TRACKING_NEITHER); //} //else { //OffsetDistOutput = PIDCalcs((double)gl_FrontCm, (float)offsetCm, lastError, // lastInput, lastIval, lastDerror, OffsetDistKp, OffsetDistKi, OffsetDistKd); OffsetDistOutput = PIDCalcs((double)gl_FrontCm, (float)offsetCm, lastError, lastInput, lastIval, lastDerror, Kp, Ki, Kd); int16_t speed = (int16_t)OffsetDistOutput; //gl_pSerPort->printf("MTFD: just after PIDCalcs, speed = %d\n", speed); //PIDCalcs out can be negative, must handle both signs if (speed >= 0) { speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : 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%d\t%2.0f\t%2.0f\n", millis(), gl_FrontCm, offsetCm, lastError, lastIval, OffsetDistOutput, speed, gl_Frontvar, gl_Rearvar); //gl_pSerPort->printf("%lu\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%d\t%2.0f\t%2.0f\n", // millis(), gl_frontcm, offsetcm, lasterror, lastival, offsetdistoutput, speed, gl_frontvar, gl_rearvar); // 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); //11/17/23 added to zero out lastIval anytime Kp*Err > max speed (MOTOR_SPEED_QTR here) //if (abs(gl_FrontCm - offsetCm)*OffsetDistKp > MOTOR_SPEED_QTR) if (abs(gl_FrontCm - offsetCm) * Kp > MOTOR_SPEED_QTR) { lastIval = 0; } } } } gl_pSerPort->printf("MTFD: Exited with front dist = %d, anomaly code = %s\n", GetFrontDistCm(), AnomalyStrArray[gl_LastAnomalyCode]);//c/o 06/11/23 - chks now done in HandleAnomalousConditions() StopBothMotors(); //show 10 distances after stop gl_pSerPort->printf("Front\tRear\tLeft\tRight\n"); for (size_t i = 0; i < 10; i++) { GetRequestedVL53l0xValues(VL53L0X_ALL); gl_FrontCm = GetFrontDistCm(); gl_pSerPort->printf("%d\t%2.2f\t%2.2f\t%2.2f\n", gl_FrontCm, gl_RearCm, gl_LeftCenterCm, gl_RightCenterCm); delay(100); } if (abs(gl_FrontCm - offsetCm) > 1) { gl_pSerPort->printf("Second time through MTDFD with error = %d\n", abs(gl_FrontCm - offsetCm)); MoveToDesiredFrontDistCm(offsetCm, Kp, Ki, Kd); } gl_LastAnomalyCode = ANOMALY_NONE; return true; } |
Here’s the telemetry from the run with recursive MoveToDesiredFrontDistCm() calls:
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 164 165 166 167 168 169 170 171 172 173 174 |
MTFD: at start, tgt = 30cm, curr_dist = 244, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 294105 243 30 -213.0 -21.3 -404.7 -75 75069 12983 294155 243 30 -213.0 -21.3 -447.3 -75 72368 12831 294205 241 30 -211.0 -21.1 -443.5 -75 70020 12694 294255 241 30 -211.0 -21.1 -443.1 -75 67994 12572 294305 238 30 -208.0 -20.8 -437.4 -75 66317 12463 294355 236 30 -206.0 -20.6 -433.0 -75 64966 12369 294407 237 30 -207.0 -20.7 -434.5 -75 63898 12289 294457 232 30 -202.0 -20.2 -425.2 -75 63170 12222 294507 231 30 -201.0 -20.1 -422.3 -75 62721 12169 294557 230 30 -200.0 -20.0 -420.2 -75 62540 12129 294607 227 30 -197.0 -19.7 -414.3 -75 62633 12101 294657 227 30 -197.0 -19.7 -413.7 -75 62950 12086 294707 225 30 -195.0 -19.5 -409.9 -75 63496 12083 294757 222 30 -192.0 -19.2 -403.8 -75 64265 12092 294807 218 30 -188.0 -18.8 -395.6 -75 65249 12112 294844 215 30 -185.0 -18.5 -389.1 -75 66416 12144 294894 215 30 -185.0 -18.5 -388.5 -75 67706 12186 294944 214 30 -184.0 -18.4 -386.6 -75 69110 12239 294994 211 30 -181.0 -18.1 -380.7 -75 70626 12302 295044 208 30 -178.0 -17.8 -374.4 -75 72229 12374 295094 203 30 -173.0 -17.3 -364.3 -75 73915 12456 295144 203 30 -173.0 -17.3 -363.3 -75 75598 12547 295194 201 30 -171.0 -17.1 -359.5 -75 77271 12646 295244 200 30 -170.0 -17.0 -357.2 -75 78892 12754 295294 197 30 -167.0 -16.7 -351.3 -75 80455 12869 295346 196 30 -166.0 -16.6 -348.8 -75 81902 12991 295396 191 30 -161.0 -16.1 -339.1 -75 83245 13121 295446 192 30 -162.0 -16.2 -340.0 -75 84383 13256 295496 191 30 -161.0 -16.1 -338.3 -75 85302 13398 295546 187 30 -157.0 -15.7 -330.5 -75 85997 13545 295596 184 30 -154.0 -15.4 -324.0 -75 86417 13697 295648 185 30 -155.0 -15.5 -325.3 -75 86484 13853 295698 179 30 -149.0 -14.9 -314.1 -75 86224 14013 295749 182 30 -152.0 -15.2 -318.6 -75 85513 14177 295798 177 30 -147.0 -14.7 -309.7 -75 84379 14344 295849 176 30 -146.0 -14.6 -306.8 -75 82743 14513 295898 173 30 -143.0 -14.3 -300.9 -75 80577 14684 295948 171 30 -141.0 -14.1 -296.5 -75 77824 14856 295998 171 30 -141.0 -14.1 -296.1 -75 74425 15029 296048 167 30 -137.0 -13.7 -288.5 -75 70358 15202 296098 163 30 -133.0 -13.3 -280.1 -75 65570 15375 296148 160 30 -130.0 -13.0 -273.6 -75 60002 15547 296198 160 30 -130.0 -13.0 -273.0 -75 53584 15718 296248 156 30 -126.0 -12.6 -265.4 -75 46285 15886 296298 155 30 -125.0 -12.5 -262.7 -75 38034 16051 296348 151 30 -121.0 -12.1 -254.9 -75 28787 16213 296398 148 30 -118.0 -11.8 -248.4 -75 18481 16371 296448 149 30 -119.0 -11.9 -249.7 -75 7046 16524 296498 147 30 -117.0 -11.7 -246.1 -75 898 16672 296548 143 30 -113.0 -11.3 -238.1 -75 913 16814 296598 140 30 -110.0 -11.0 -231.6 -75 928 16949 296648 138 30 -108.0 -10.8 -227.2 -75 939 17077 296698 135 30 -105.0 -10.5 -221.1 -75 952 17197 296748 134 30 -104.0 -10.4 -218.6 -75 958 17308 296798 129 30 -99.0 -9.9 -208.9 -75 972 17410 296848 128 30 -98.0 -9.8 -206.0 -75 983 17501 296898 127 30 -97.0 -9.7 -203.9 -75 985 17582 296948 123 30 -93.0 -9.3 -196.1 -75 997 17650 296998 122 30 -92.0 -9.2 -193.4 -75 1003 17707 297048 120 30 -90.0 -9.0 -189.4 -75 1007 17750 297098 117 30 -87.0 -8.7 -183.3 -75 1014 17779 297148 114 30 -84.0 -8.4 -177.0 -75 1018 17794 297200 112 30 -82.0 -8.2 -172.6 -75 1021 17793 297250 112 30 -82.0 -8.2 -172.2 -75 1020 17776 297300 108 30 -78.0 -7.8 -164.6 -75 1028 17742 297350 105 30 -75.0 -7.5 -158.1 -75 1039 17689 297400 101 30 -71.0 -7.1 -149.9 -75 1049 17618 297450 101 30 -71.0 -7.1 -149.1 -75 1051 17528 297500 98 30 -68.0 -6.8 -143.4 -75 1057 17417 297550 96 30 -66.0 -6.6 -139.0 -75 1064 17285 297600 94 30 -64.0 -6.4 -134.8 -75 1076 17130 297650 91 30 -61.0 -6.1 -128.7 -75 1085 16953 297700 89 30 -59.0 -5.9 -124.3 -75 1093 16751 297750 88 30 -58.0 -5.8 -122.0 -75 1096 16525 297800 87 30 -57.0 -5.7 -119.9 -75 1097 16273 297850 85 30 -55.0 -5.5 -115.9 -75 1096 15995 297900 84 30 -54.0 -5.4 -113.6 -75 1098 15689 297950 82 30 -52.0 -5.2 -109.6 -75 1092 15354 298000 80 30 -50.0 -5.0 -105.4 -75 1084 14990 298050 75 30 -45.0 -4.5 -95.5 -75 1086 14596 298100 74 30 -44.0 -4.4 -92.6 -75 1086 14171 298151 72 30 -42.0 -4.2 -88.6 -75 1079 13713 298201 69 30 -39.0 -3.9 -82.5 -75 1082 13221 298250 66 30 -36.0 -3.6 -76.2 -75 1075 12696 298301 64 30 -34.0 -7.0 -75.4 -75 1073 12135 298350 62 30 -32.0 -10.2 -74.6 -74 1068 11538 298400 60 30 -30.0 -13.2 -73.6 -73 1064 10904 298450 58 30 -28.0 -16.0 -72.4 -72 1058 10231 298500 57 30 -27.0 -18.7 -72.9 -72 1045 9519 298551 55 30 -25.0 -21.2 -71.6 -71 1035 8767 298600 53 30 -23.0 -23.5 -69.9 -69 1029 7973 298651 50 30 -20.0 -25.5 -66.1 -66 1026 7137 298700 48 30 -18.0 -27.3 -63.7 -63 1018 6257 298751 45 30 -15.0 -28.8 -59.4 -59 1015 5332 298800 42 30 -12.0 -30.0 -54.6 -54 1012 4362 298850 40 30 -10.0 -31.0 -51.4 -51 1012 3345 298900 39 30 -9.0 -31.9 -50.1 -50 1010 2279 298951 36 30 -6.0 -32.5 -45.1 -45 1004 1165 299001 34 30 -4.0 -32.9 -41.3 -41 996 0 299051 32 30 -2.0 -33.1 -37.5 -37 992 0 299099 30 30 0.0 -33.1 -33.5 -33 989 0 MTFD: Exited with front dist = 30, anomaly code = WALL_OFFSET_DIST_AHEAD Front Rear Left Right 29 53.80 80.00 5.30 26 53.80 80.00 5.30 24 53.80 80.00 5.30 24 53.80 80.00 5.30 23 53.80 80.00 5.30 23 53.80 80.00 5.30 25 53.80 80.00 5.30 25 53.80 80.00 5.30 25 53.80 80.00 5.30 25 53.80 80.00 5.30 Second time through MTDFD with error = 5 MTFD: at start, tgt = 30cm, curr_dist = 25, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 300310 24 30 6.0 0.6 11.4 11 83116 12983 300360 24 30 6.0 1.2 13.2 13 83681 12831 300410 25 30 5.0 1.7 11.9 11 84568 12694 300460 25 30 5.0 2.2 12.2 12 85780 12572 300510 24 30 6.0 2.8 14.6 14 87315 12463 300560 25 30 5.0 3.3 13.5 13 89115 12369 300610 25 30 5.0 3.8 13.8 13 91178 12289 300660 25 30 5.0 4.3 14.3 14 93482 12222 300710 24 30 6.0 4.9 16.7 16 96022 12169 300760 23 30 7.0 5.6 19.4 19 98772 12129 300810 24 30 6.0 6.2 18.4 18 101673 12101 300860 24 30 6.0 6.8 18.8 18 104715 12086 300910 25 30 5.0 7.3 17.5 17 107854 12083 300960 24 30 6.0 7.9 19.7 19 111097 12092 301010 23 30 7.0 8.6 22.4 22 114416 12112 301060 24 30 6.0 9.2 21.4 21 117744 12144 301110 25 30 5.0 9.7 19.9 19 121053 12186 301160 23 30 7.0 10.4 24.0 24 124359 12239 301210 24 30 6.0 11.0 23.2 23 127581 12302 301260 25 30 5.0 11.5 21.7 21 130685 12374 301310 25 30 5.0 12.0 22.0 22 133654 12456 301360 25 30 5.0 12.5 22.5 22 136450 12547 301410 25 30 5.0 13.0 23.0 23 139039 12646 301463 25 30 5.0 13.5 23.5 23 141381 12754 301513 25 30 5.0 14.0 24.0 24 143440 12869 301563 25 30 5.0 14.5 24.5 24 145174 12991 301613 25 30 5.0 15.0 25.0 25 146543 13121 301663 26 30 4.0 15.4 23.6 23 147494 13256 301713 27 30 3.0 15.7 21.9 21 147986 13398 301763 27 30 3.0 16.0 22.0 22 147988 13545 301813 28 30 2.0 16.2 20.4 20 147445 13697 301863 28 30 2.0 16.4 20.4 20 146325 13853 301913 29 30 1.0 16.5 18.7 18 144569 14013 MTFD: Exited with front dist = 29, anomaly code = WALL_OFFSET_DIST_AHEAD Front Rear Left Right 29 53.80 80.00 5.30 29 53.80 80.00 5.30 28 53.80 80.00 5.30 28 53.80 80.00 5.30 28 53.80 80.00 5.30 28 53.80 80.00 5.30 29 53.80 80.00 5.30 28 53.80 80.00 5.30 29 53.80 80.00 5.30 29 53.80 80.00 5.30 Front Rear Left Right 28 53.80 80.00 5.30 29 53.80 80.00 5.30 30 53.80 80.00 5.30 29 53.80 80.00 5.30 28 53.80 80.00 5.30 29 53.80 80.00 5.30 29 53.80 80.00 5.30 29 53.80 80.00 5.30 30 53.80 80.00 5.30 29 53.80 80.00 5.30 |
The first run through MoveToDesiredFrontDistCm() occurs normally, with the exit at the 30cm target distance as expected, and then the expected overshoot to 25cm. Then the second call to MoveToDesiredFrontDistCm() starts at 24cm and gets to 29cm in approximately 1.5sec, followed by two more 10-item distance displays. I guess the first distance display is from the one inside the MoveToDesiredFrontDistCm() call, and the second one is the one from the test harness. I need the delay caused by the 10-distance display, but I can probably replace it with just a 1-sec delay.
Here’s another run with the loop replaced by a 1-sec delay:
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 |
MTFD: at start, tgt = 30cm, curr_dist = 233, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 47939 236 30 -206.0 -20.6 -391.4 -75 75287 12935 47989 234 30 -204.0 -20.4 -428.8 -75 72687 12757 48039 231 30 -201.0 -20.1 -422.7 -75 70454 12594 48089 234 30 -204.0 -20.4 -427.8 -75 68511 12446 48139 229 30 -199.0 -19.9 -418.9 -75 66940 12311 48189 227 30 -197.0 -19.7 -414.1 -75 65697 12188 48227 227 30 -197.0 -19.7 -413.7 -75 64749 12079 48277 224 30 -194.0 -19.4 -408.0 -75 64118 11983 48339 221 30 -191.0 -19.1 -401.7 -75 63791 11901 48376 220 30 -190.0 -19.0 -399.2 -75 63729 11826 48426 218 30 -188.0 -18.8 -395.2 -75 63930 11758 48476 218 30 -188.0 -18.8 -394.8 -75 64351 11700 48527 215 30 -185.0 -18.5 -389.1 -75 65013 11649 48576 215 30 -185.0 -18.5 -388.5 -75 65858 11604 48626 208 30 -178.0 -17.8 -375.2 -75 66953 11568 48676 209 30 -179.0 -17.9 -375.7 -75 68178 11542 48726 207 30 -177.0 -17.7 -372.1 -75 69547 11523 48776 202 30 -172.0 -17.2 -362.2 -75 71074 11510 48826 199 30 -169.0 -16.9 -355.5 -75 72708 11505 48876 199 30 -169.0 -16.9 -354.9 -75 74387 11505 48926 197 30 -167.0 -16.7 -351.1 -75 76108 11507 48979 196 30 -166.0 -16.6 -348.8 -75 77831 11514 49029 194 30 -164.0 -16.4 -344.8 -75 79538 11524 49079 192 30 -162.0 -16.2 -340.6 -75 81200 11537 49128 189 30 -159.0 -15.9 -334.5 -75 82794 11552 49181 189 30 -159.0 -15.9 -333.9 -75 84256 11571 49229 183 30 -153.0 -15.3 -322.5 -75 85616 11593 49281 184 30 -154.0 -15.4 -323.2 -75 86761 11618 49330 181 30 -151.0 -15.1 -317.7 -75 87701 11649 49381 179 30 -149.0 -14.9 -313.3 -75 88385 11682 49431 177 30 -147.0 -14.7 -309.1 -75 88775 11719 49481 175 30 -145.0 -14.5 -304.9 -75 88830 11759 49531 175 30 -145.0 -14.5 -304.5 -75 88493 11802 49581 172 30 -142.0 -14.2 -298.8 -75 87747 11849 49630 171 30 -141.0 -14.1 -296.3 -75 86533 11897 49680 167 30 -137.0 -13.7 -288.5 -75 84831 11946 49731 168 30 -138.0 -13.8 -289.6 -75 82555 11995 49780 163 30 -133.0 -13.3 -280.3 -75 79704 12045 49830 163 30 -133.0 -13.3 -279.3 -75 76193 12093 49880 157 30 -127.0 -12.7 -267.9 -75 72013 12143 49930 158 30 -128.0 -12.8 -268.6 -75 67068 12192 49980 155 30 -125.0 -12.5 -263.1 -75 61330 12240 50030 154 30 -124.0 -12.4 -260.6 -75 54737 12289 50080 150 30 -120.0 -12.0 -252.8 -75 47249 12336 50130 148 30 -118.0 -11.8 -248.2 -75 38800 12381 50180 146 30 -116.0 -11.6 -244.0 -75 29333 12425 50230 141 30 -111.0 -11.1 -234.1 -75 18800 12466 50280 142 30 -112.0 -11.2 -235.0 -75 7123 12505 50330 138 30 -108.0 -10.8 -227.6 -75 853 12540 50380 134 30 -104.0 -10.4 -219.2 -75 871 12572 50430 134 30 -104.0 -10.4 -218.4 -75 880 12600 50480 131 30 -101.0 -10.1 -212.7 -75 891 12622 50530 129 30 -99.0 -9.9 -208.3 -75 903 12641 50580 127 30 -97.0 -9.7 -204.1 -75 905 12655 50630 125 30 -95.0 -9.5 -199.9 -75 913 12664 50682 122 30 -92.0 -9.2 -193.8 -75 923 12668 50730 120 30 -90.0 -9.0 -189.4 -75 928 12667 50782 118 30 -88.0 -8.8 -185.2 -75 935 12660 50832 116 30 -86.0 -8.6 -181.0 -75 943 12646 50882 114 30 -84.0 -8.4 -176.8 -75 949 12625 50932 113 30 -83.0 -8.3 -174.5 -75 952 12599 50982 110 30 -80.0 -8.0 -168.6 -75 952 12565 51032 109 30 -79.0 -7.9 -166.1 -75 952 12523 51082 107 30 -77.0 -7.7 -162.1 -75 947 12474 51132 104 30 -74.0 -7.4 -156.0 -75 954 12417 51182 103 30 -73.0 -7.3 -153.5 -75 952 12351 51232 100 30 -70.0 -7.0 -147.6 -75 952 12277 51282 96 30 -66.0 -6.6 -139.4 -75 962 12195 51332 93 30 -63.0 -6.3 -132.9 -75 976 12103 51382 93 30 -63.0 -6.3 -132.3 -75 980 12001 51432 90 30 -60.0 -6.0 -126.6 -75 987 11890 51482 91 30 -61.0 -6.1 -127.9 -75 984 11768 51532 88 30 -58.0 -5.8 -122.4 -75 983 11637 51582 88 30 -58.0 -5.8 -121.8 -75 977 11495 51632 83 30 -53.0 -5.3 -112.3 -75 979 11341 51682 81 30 -51.0 -5.1 -107.5 -75 977 11178 51732 78 30 -48.0 -4.8 -101.4 -75 984 11001 51782 76 30 -46.0 -4.6 -97.0 -75 985 10814 51833 75 30 -45.0 -4.5 -94.7 -75 985 10613 51883 74 30 -44.0 -4.4 -92.6 -75 982 10399 51933 71 30 -41.0 -4.1 -86.7 -75 981 10172 51983 69 30 -39.0 -3.9 -82.3 -75 980 9930 52033 66 30 -36.0 -3.6 -76.2 -75 976 9675 52083 63 30 -33.0 -6.9 -73.5 -73 975 9406 52133 62 30 -32.0 -10.1 -74.3 -74 969 9125 52183 61 30 -31.0 -13.2 -75.4 -75 965 8830 52233 59 30 -29.0 -16.1 -74.5 -74 953 8520 52283 57 30 -27.0 -18.8 -73.2 -73 948 8195 52333 53 30 -23.0 -21.1 -67.9 -67 941 7854 52383 50 30 -20.0 -23.1 -63.7 -63 945 7496 52433 47 30 -17.0 -24.8 -59.4 -59 944 7122 52483 45 30 -15.0 -26.3 -56.7 -56 943 6730 52533 44 30 -14.0 -27.7 -55.9 -55 938 6320 52583 41 30 -11.0 -28.8 -51.4 -51 938 5891 52632 40 30 -10.0 -29.8 -50.0 -50 936 5441 52683 38 30 -8.0 -30.6 -47.0 -47 933 4976 52733 37 30 -7.0 -31.3 -45.5 -45 933 4492 52783 34 30 -4.0 -31.7 -40.3 -40 929 3990 52833 33 30 -3.0 -32.0 -38.2 -38 927 3464 52883 33 30 -3.0 -32.3 -38.3 -38 924 3516 52931 30 30 0.0 -32.3 -32.9 -32 919 3558 MTFD: Exited with front dist = 28, anomaly code = WALL_OFFSET_DIST_AHEAD delaying 1sec to allow distances to settle Front Rear Left Right 24 260.40 153.10 78.60 Second time through MTDFD with error = 6 MTFD: at start, tgt = 30cm, curr_dist = 23, front/rear var = 83325/13332 Msec FrontD TgtD Err Ival Output speed FrontVar RearVar 54153 24 30 6.0 0.6 11.4 11 83135 12618 54203 24 30 6.0 1.2 13.2 13 83699 12263 54253 24 30 6.0 1.8 13.8 13 84606 11914 54303 25 30 5.0 2.3 12.5 12 85817 11565 54353 25 30 5.0 2.8 12.8 12 87333 11223 54403 25 30 5.0 3.3 13.3 13 89133 10882 54453 25 30 5.0 3.8 13.8 13 91197 10547 54503 24 30 6.0 4.4 16.2 16 93519 10217 54553 25 30 5.0 4.9 15.1 15 96040 9890 54603 25 30 5.0 5.4 15.4 15 98754 9569 54653 24 30 6.0 6.0 17.8 17 101655 9251 54703 24 30 6.0 6.6 18.6 18 104697 8938 54753 24 30 6.0 7.2 19.2 19 107854 8632 54803 23 30 7.0 7.9 21.7 21 111115 8331 54853 24 30 6.0 8.5 20.7 20 114416 8035 54903 25 30 5.0 9.0 19.2 19 117727 7745 54953 24 30 6.0 9.6 21.4 21 121053 7463 55003 25 30 5.0 10.1 20.3 20 124327 7186 55053 26 30 4.0 10.5 18.7 18 127517 6915 55103 25 30 5.0 11.0 20.8 20 130623 6650 55153 26 30 4.0 11.4 19.6 19 133577 6392 55203 26 30 4.0 11.8 19.8 19 136361 6140 55253 25 30 5.0 12.3 22.1 22 138951 5896 55303 24 30 6.0 12.9 24.7 24 141310 5658 55353 24 30 6.0 13.5 25.5 25 143384 5428 55403 25 30 5.0 14.0 24.2 24 145120 5204 55453 25 30 5.0 14.5 24.5 24 146491 4987 55503 26 30 4.0 14.9 23.1 23 147444 4778 55553 24 30 6.0 15.5 27.1 27 147974 4575 55606 25 30 5.0 16.0 26.2 26 148000 4380 55656 25 30 5.0 16.5 26.5 26 147490 4192 55706 25 30 5.0 17.0 27.0 27 146398 4011 55756 25 30 5.0 17.5 27.5 27 144680 3838 55806 25 30 5.0 18.0 28.0 28 142286 3672 55856 25 30 5.0 18.5 28.5 28 139170 3513 55906 27 30 3.0 18.8 25.2 25 135265 3362 55956 27 30 3.0 19.1 25.1 25 130540 3218 56006 29 30 1.0 19.2 21.6 21 124931 3081 MTFD: Exited with front dist = 29, anomaly code = WALL_OFFSET_DIST_AHEAD delaying 1sec to allow distances to settle Front Rear Left Right 30 257.50 152.30 78.90 58131: bottom of FrontBackMotionTest 'while' loop 58131: Top of FrontBackMotionTest 'while' loop |
From the above, it looks like the function is doing what it should. It keeps iterating until the distance measured after a 1-sec delay fits within the +/- 1cm window.
MoveToDesiredRearDistCm():
Now that MoveToDesiredFrontDistCm() is working, I started on MoveToDesiredRearDistCm(). I started by copying MoveToDesiredFrontDistCm() and changing all relevant occurrences of ‘Front’ to ‘Rear’. This was about 99% of the required effort. However, there were two issues that surfaced during the port. The first was that the test program needed the ability to pass the PID values into the function, but all the mainline code uses just a single parameter (the desired offset). As a temporary fix I created a four-parameter version of the function with all the code, and had the single parameter version pass the global PID values to the four-parameter on. However a much better solution was to remove the single parameter version of MoveToDesiredFrontDistCm() entirely, and instead declare the four-parameter version with three default values right after the definition, as shown here:
1 2 3 4 5 6 |
//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/ const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode const float OffsetDistKi = 0.1f; const float OffsetDistKd = 0.2f; bool MoveToDesiredFrontDistCm(uint16_t offsetCm, float Kp = OffsetDistKp, float Ki = OffsetDistKi, float Kd = OffsetDistKd);//11/19/23 put here to provide default vals bool MoveToDesiredRearDistCm(uint16_t offsetCm, float Kp = OffsetDistKp, float Ki = OffsetDistKi, float Kd = OffsetDistKd);//11/19/23 put here to provide default vals |
The second issue was the sign of the speed variable in the RunBothMotorsBidirectional() function. For MoveToDesiredFrontDistCm() the parameters to this function had to both be negative, as in RunBothMotorsBidirectional(-speed, -speed), but in the MoveToDesiredRearDistCm() they have to be positive as in RunBothMotorsBidirectional(speed, speed). Once this was accomplished, the MoveToDesiredRearDistCm() operation was successful.
MoveToDesiredLeft/RightDistCm():
I started on these two functions, but soon realized that these two functions aren’t called anymore. They were originally used as part of wall offset capture operations, but were abandoned in favor of an algorithm that uses a perpendicular approach. Soooo, I will just remove these two functions entirely and call it good!
Stay tuned,
Frank