Posted 11 November 2023
After getting the wide-body IR homing PID values defined properly, the next challenge was to actually get the robot to connect to the charging station, and after getting charged, to disconnect from it successfully. After the usual number of errors and goofs, I believe I have it working now. Here is a short video and the telemetry for a complete walltracking – IR home to charging station – disconnect from charging station – back to walltracking cycle
And here is the telemetry for the 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 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 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 |
Opening port Port open gl_pSerPort now points to active Serial (USB or Wixel) 7950: Starting setup() for WallE3_Quicksort_V4.ino Checking for MPU6050 IMU at I2C Addr 0x68 MPU6050 connection successful Initializing DMP... Enabling DMP... DMP ready! Waiting for MPU6050 drift rate to settle... Calibrating...Retrieving Calibration Values Msec Hdg 10580 0.101 0.098 MPU6050 Ready at 10.58 Sec with delta = 0.003 Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x20 Teensy available at 11681 with gl_bVL53L0X_TeensyReady = 1. Waiting for Teensy setup() to finish 11684: got 1 from VL53L0X Teensy Teensy setup() finished at 11786 mSec VL53L0X Teensy Ready at 11788 Checking for Garmin LIDAR at Wire2 I2C addr 0x62 LIDAR Responded to query at Wire2 address 0x62! Setting LIDAR acquisition repeat value to 0x02 for <100mSec measurement repeat delay Initializing Front Distance Array...Done Initializing Rear Distance Array...Done Initializing Left/Right Distance Arrays...Done Checking for Teensy 3.2 IRDET Controller at I2C addr 0x8 11837: IRDET Teensy Not Avail... IRDET Teensy Ready at 11940 Fin1/Fin2/SteeringVal = 7960 12984 -0.2402 11941: Initializing IR Beam Total Value Averaging Array...Done 11945: Initializing IR Beam Steering Value Averaging Array...Done Battery Voltage = 7.36 14153: End of setup(): Elapsed run time set to 0 0.0: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.37 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 0.4: gl_Left/RightCenterCm = 125.2/23.8, Left/RightSteerVal = -0.38/0.00 Just before call to HandleAnomalousConditions() with anomaly code = NONE In HandleAnomalousConditions(NEITHER) with last anomaly code = NONE In HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE 0.5: gl_Left/RightCenterCm = 124.9/23.8, Left/RightSteerVal = -0.23/0.17 TrackRightWallOffset(350.0, 0.0, 20.0, 30) called TrackRightWallOffset: Start tracking offset of 30cm at 0.5 Just after CaptureWallOffset(TRACKING_RIGHT, 23.8) Sec LCen RCen Deg RF RR RStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.6 125.8 23.4 0.2 23.7 22.8 -0.04 182 55 74880 12882 15 127 NONE RIGHT 0.6 126.0 23.8 -0.0 22.4 22.8 0.09 181 54 73274 12742 54 95 NONE RIGHT 0.7 125.2 23.3 -1.9 24.2 22.7 0.15 183 54 71972 12616 70 79 NONE RIGHT 0.7 125.6 23.5 -3.7 22.3 22.7 -0.04 182 58 71002 12497 15 127 NONE RIGHT 0.8 136.9 23.6 -5.2 24.3 22.8 0.15 181 64 70351 12374 68 81 NONE RIGHT 0.8 145.2 24.1 -7.2 22.8 22.4 0.04 181 68 69993 12252 48 101 NONE RIGHT 0.9 150.7 24.0 -8.8 25.7 23.1 0.26 180 89 69925 12093 106 43 NONE RIGHT 0.9 155.0 23.8 -10.1 23.8 22.7 0.11 179 100 70133 11924 67 82 NONE RIGHT 1.0 158.5 24.7 -10.1 24.4 23.2 0.12 178 106 70599 11754 67 82 NONE RIGHT 1.0 154.7 23.0 -10.0 24.8 23.5 0.13 175 106 71333 11595 64 85 NONE RIGHT 1.1 143.6 23.9 -10.0 24.7 23.9 0.08 173 106 72301 11450 54 95 NONE RIGHT 1.1 136.0 23.8 -10.0 25.3 23.9 0.14 170 106 73497 11316 67 82 IR_BEAM_AVAILABLE RIGHT Stopping Motors! Error Code is 9 --> IR_BEAM_AVAILABLE 1.1: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.35 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 1.6: gl_Left/RightCenterCm = 119.5/25.5, Left/RightSteerVal = -0.30/0.10 Just before call to HandleAnomalousConditions() with anomaly code = IR_BEAM_AVAILABLE In HandleAnomalousConditions(RIGHT) with last anomaly code = IR_BEAM_AVAILABLE IR Beam Detected with IR Beam Av = 32096, Battery Voltage = 7.35 Low battery - homing to Charger OK we have a IR Beacon, with front distance = 158, LRSteer = -0.7! In IRHomingNavigateToIAP with LF/LR/LS/RF/RR/RS = 121.0 121.6 -0.06 27.2 26.3 0.09 In IRHomingNavigateToIAP(RIGHT) with LRSteer = -0.6 In TurnToHomingBeacon(CW) with LRSteer = -0.6 At start: beaconAvg = 35847, deg = -9.1 with front dist = 160 Msec Deg Curr Prev Avg 17789 -9.1 -0.6 -0.6 35830 17896 -5.8 -0.6 -0.6 35614 18010 2.1 -0.4 -0.6 35870 18117 11.1 -0.1 -0.4 35999 18229 20.6 0.3 -0.1 33370 18346: Coarse tuning loop exited at 30.6deg with avg = 33370 and frontdist = 52 Fine Tuning: TurnToHomingBeacon(CW) with IRAvg = 30301, Steering = 0.62 & IRRotate Kp,Ki,Kd = (40, 5, 0) TTHB Fine Tune Start: Got 21660, 5108, 30301, 0.62 from IR Detector module Msec IRVal1 IRVal2 ValAvg Steer speed 19542 20630 2364 23020 0.79 -35 19692 20954 2546 23171 0.78 -39 19842 22392 3620 24168 0.72 -40 19993 21312 6060 25628 0.56 -36 20145 21514 10090 28329 0.36 -30 20293 19188 15268 31144 0.11 -21 20443 17170 19046 34092 -0.05 -14 20593 16724 19878 35758 -0.09 -12 20743 16716 19796 36443 -0.08 -12 20893 16680 19804 36532 -0.09 -11 21043 16672 19776 36481 -0.09 -11 21193 16642 19814 36462 -0.09 -10 21343 16576 19844 36441 -0.09 -10 21493 16606 19768 36416 -0.09 -9 21643 16586 19736 36372 -0.09 -9 21793 16596 19718 36336 -0.09 -9 21943 16562 19680 36292 -0.09 -8 22093 16516 19702 36258 -0.09 -8 22243 16534 19628 36207 -0.09 -7 22393 16520 19610 36170 -0.09 -7 22543 16406 19626 36108 -0.09 -6 22693 16344 19544 36016 -0.09 -6 22843 16320 19508 35916 -0.09 -5 22993 16286 19462 35821 -0.09 -5 23143 16206 19456 35746 -0.09 -4 23293 16146 19414 35656 -0.09 -4 23443 16046 19306 35524 -0.09 -3 23593 15946 19206 35354 -0.09 -3 23743 15722 19062 35096 -0.10 -2 23893 15548 18950 34811 -0.10 -2 24043 15322 18694 34432 -0.10 -1 24193 14950 18442 33968 -0.10 0 24343 15248 18188 33614 -0.09 -1 24493 15832 18278 33646 -0.07 -1 24643 16518 19136 34400 -0.07 -1 24793 16820 19940 35508 -0.08 0 24943 16788 19980 36394 -0.09 0 25093 16762 20016 36768 -0.09 0 25243 16818 19998 36787 -0.09 1 25393 16810 20024 36809 -0.09 1 25543 16840 19968 36819 -0.08 1 25693 16730 20046 36806 -0.09 2 25843 16698 20060 36780 -0.09 3 25993 16790 19936 36753 -0.09 3 26143 16736 19960 36726 -0.09 3 26293 16742 19960 36708 -0.09 4 26443 16806 19890 36698 -0.08 4 26593 16738 19890 36675 -0.09 5 26743 16704 19902 36643 -0.09 5 26893 16738 19884 36618 -0.09 5 27043 16632 19922 36594 -0.09 6 27193 16680 19840 36565 -0.09 6 27343 16690 19836 36533 -0.09 7 27493 16670 19806 36507 -0.09 7 27643 16622 19808 36477 -0.09 8 27793 16640 19788 36444 -0.09 8 27943 16644 19774 36425 -0.09 8 28093 16580 19798 36408 -0.09 9 28243 16562 19804 36387 -0.09 10 28393 16552 19746 36347 -0.09 10 28543 16544 19712 36306 -0.09 10 28693 16468 19676 36232 -0.09 11 28843 16500 19608 36169 -0.09 11 28993 16466 19578 36098 -0.09 12 29143 16428 19586 36055 -0.09 12 29293 16396 19602 36018 -0.09 13 29443 16336 19586 35978 -0.09 13 29593 16272 19466 35886 -0.09 13 29743 16290 19474 35808 -0.09 14 29893 16174 19358 35678 -0.09 14 30043 16084 19284 35554 -0.09 15 30193 15912 19224 35345 -0.09 16 30343 15772 19130 35135 -0.10 16 30493 15670 18910 34872 -0.09 16 30643 15458 18480 34473 -0.09 17 30793 15254 18000 33924 -0.08 17 30943 15268 17494 33318 -0.07 17 31093 15764 17440 33073 -0.05 16 31243 16480 18066 33504 -0.05 16 31393 17054 18920 34574 -0.05 17 TTHB Fine Tune End: 17072, 18994, 35528, -0.05 from IR Detector module 31422: start/end_deg = -9.1/7.2, start/end_dist = 160/158 NavToIAP: IP is at 45 cm offset, 135 cm away from charging station, a ratio of 0.3 NavToIAP: Robot is at 159 from charger, so we need a 52 cm offset In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 27.24/27.98/-0.07 NavToIAP(RIGHT) with par/IR hdg deg = 7.2/7.2, par/IR dist = 157/159 NavToIAP(RIGHT) with LF/LR/LS/RF/RR/RS = 155.4 154.3 0.1 26.5 28.1 -0.2 NavToIAP: Robot on same wall as charging station - proceed with normal homing NavToIAP: Current wall offset is 26.8 cm NavToIAP: After SpinTurn(), rear dist (cm) is 27.0 In MoveToDesiredRearDistCm(52) with Kp/Ki/Kd = 1.50/0.10/0.20 MTRD: at start, tgt = 52cm, curr_dist = 27, front/rear var = 83325/13332 Msec Rdist TgtD err Ival Out Speed FVar RVar 35709 26.6 52 25.4 2.5 35.6 35 77266 13090 35759 26.2 52 25.8 5.1 43.7 43 75949 13039 35809 25.9 52 26.1 7.7 46.8 46 74999 13007 35859 26.1 52 25.9 10.3 49.2 49 74361 12986 35909 26.7 52 25.3 12.8 50.9 50 74049 12979 35959 27.5 52 24.5 15.3 52.2 52 74034 12983 36009 28.5 52 23.5 17.6 53.1 53 74343 12996 36059 30.9 52 21.1 19.8 51.9 51 74902 13016 36109 32.4 52 19.6 21.7 51.4 51 75722 13042 36159 32.8 52 19.2 23.6 52.5 52 76826 13081 36209 34.7 52 17.3 25.4 51.7 51 78151 13125 36259 35.7 52 16.3 27.0 51.6 51 79704 13177 36309 37.2 52 14.8 28.5 51.0 50 81405 13235 36359 38.9 52 13.1 29.8 49.8 49 83318 13301 36409 41.0 52 11.0 30.9 47.8 47 85315 13368 36459 42.5 52 9.5 31.8 46.4 46 87429 13442 36509 44.2 52 7.8 32.6 44.7 44 89662 13520 36559 45.9 52 6.1 33.2 42.7 42 91914 13604 36611 47.4 52 4.6 33.7 40.9 40 94239 13692 36659 48.4 52 3.6 34.0 39.6 39 96498 13786 36711 51.4 52 0.6 34.1 35.6 35 98742 13880 MTRD: Stopped with rear dist = 51, anomaly code = IR_BEAM_AVAILABLE MTRD: Stopped with rear dist = 51 IDX FrontD RearD 0 460 92 1 470 96 2 480 100 3 490 104 4 500 108 5 510 112 6 520 116 7 530 120 8 540 124 9 550 128 10 560 132 11 570 136 12 580 140 13 590 144 14 600 148 15 610 152 16 620 156 17 630 160 18 640 164 19 650 168 20 660 172 21 670 176 22 680 180 23 690 184 24 700 188 25 710 192 26 720 196 27 730 200 28 740 204 29 750 208 30 760 212 31 770 216 32 780 220 33 790 224 34 800 228 35 810 232 36 820 236 37 830 240 38 840 244 39 850 248 40 860 252 41 870 256 42 880 260 43 890 264 44 900 268 45 910 272 46 920 276 47 930 280 48 940 284 49 950 288 50 960 292 51 970 296 52 980 300 53 990 304 54 147 308 55 147 312 56 148 316 57 148 320 58 146 324 59 146 328 60 148 332 61 148 336 62 146 340 63 146 344 64 147 348 65 147 352 66 146 356 67 146 360 68 146 364 69 146 368 70 143 372 71 143 376 72 144 380 73 144 384 74 143 388 75 143 392 76 139 396 77 139 26 78 138 26 79 138 26 80 135 26 81 135 25 82 136 26 83 136 26 84 131 27 85 131 28 86 133 30 87 133 32 88 131 32 89 131 34 90 127 35 91 127 37 92 128 38 93 128 41 94 123 42 95 123 44 96 126 45 97 126 47 98 123 48 99 123 51 In TurnToHomingBeacon(CW) with LRSteer = -0.3 At start: beaconAvg = 102, deg = -85.0 with front dist = 118 Msec Deg Curr Prev Avg 36849 -84.9 0.0 -0.3 101 36956 -81.3 0.2 0.0 73 37070 -73.3 -0.1 0.2 80 37177 -64.2 0.0 -0.1 107 37288 -54.8 0.5 0.0 97 37400 -45.2 -1.0 0.5 8626 37507 -35.4 -1.0 -1.0 23297 37619 -25.5 -0.9 -1.0 32146 37731 -15.5 -0.9 -0.9 34919 37837 -5.4 -0.8 -0.9 37778 37949 4.8 -0.6 -0.8 42014 38061 15.0 -0.4 -0.6 47425 38168 25.3 0.1 -0.4 51514 38288: Coarse tuning loop exited at 35.3deg with avg = 51514 and frontdist = 102 Fine Tuning: TurnToHomingBeacon(CW) with IRAvg = 51268, Steering = 0.50 & IRRotate Kp,Ki,Kd = (40, 5, 0) TTHB Fine Tune Start: Got 36464, 12168, 51268, 0.50 from IR Detector module Msec IRVal1 IRVal2 ValAvg Steer speed 39484 40034 5894 45930 0.74 -33 39634 39880 6080 45952 0.74 -36 39785 37278 7982 45716 0.65 -36 39936 36324 11054 46199 0.53 -34 40085 34530 16074 47747 0.36 -29 40235 28236 23308 49842 0.10 -19 40385 23368 28180 51232 -0.09 -11 40535 21976 29310 51459 -0.14 -8 40685 21966 29254 51351 -0.14 -8 40835 21944 29236 51228 -0.14 -7 40985 21906 29202 51169 -0.14 -6 41135 21920 29244 51150 -0.14 -5 41285 21852 29214 51112 -0.14 -5 41435 21936 29118 51094 -0.14 -4 41585 21930 29204 51084 -0.14 -3 41735 21834 29148 51056 -0.14 -2 41885 21850 29054 51006 -0.14 -2 42035 21788 29132 50935 -0.14 -1 42185 21752 29040 50872 -0.14 0 42335 21716 29068 50832 -0.14 0 42485 21696 29036 50769 -0.14 0 42635 21676 28982 50724 -0.14 1 42785 21652 28966 50669 -0.14 2 42935 21666 28874 50605 -0.14 2 43085 21576 28862 50532 -0.14 3 43235 21504 28886 50456 -0.15 4 43385 21438 28826 50364 -0.15 5 43535 21330 28726 50236 -0.15 5 43685 21260 28600 50060 -0.15 6 43835 21048 28562 49842 -0.15 7 43985 20870 28432 49590 -0.15 8 44135 20634 28246 49264 -0.16 9 44285 20264 28034 48826 -0.16 10 44435 20440 27684 48434 -0.15 10 44585 21348 27430 48400 -0.12 10 44735 22118 29138 49386 -0.14 11 44885 22136 29468 50546 -0.14 12 45035 22152 29392 51468 -0.14 12 45185 22154 29416 51572 -0.14 13 45335 22082 29398 51531 -0.14 14 45485 22100 29426 51525 -0.14 15 45635 22120 29474 51533 -0.14 15 45785 22218 29298 51545 -0.14 16 45935 22750 28972 51610 -0.12 16 46085 22986 28770 51664 -0.11 16 46235 22998 28710 51728 -0.11 16 46385 22926 28764 51718 -0.11 17 46535 22884 28760 51680 -0.11 18 46685 22884 28756 51658 -0.11 18 46835 22894 28680 51619 -0.11 19 46985 22974 28570 51586 -0.11 19 47135 22996 28560 51558 -0.11 20 47285 22984 28532 51538 -0.11 20 47435 23080 28492 51548 -0.10 21 47585 23342 28072 51500 -0.09 21 47735 23752 27892 51543 -0.08 20 47885 23766 27742 51522 -0.08 21 48035 23820 27744 51572 -0.08 21 48185 24252 27370 51564 -0.06 21 48335 24444 27210 51613 -0.05 21 48485 24496 27108 51626 -0.05 21 48635 24566 27006 51610 -0.05 21 TTHB Fine Tune End: 24584, 26992, 51584, -0.05 from IR Detector module 48659: start/end_deg = -85.0/17.6, start/end_dist = 118/162 NavToIAP: Wall offset now 168.5 cm, with IRHomingValTotalAvg = 51564 NavToIAP: Should be close to beacon centerline val avg = 51564, SteeringVal = -0.047 IRHomeToChgStn with detection value = 51518, Steering = -0.05 & IRHOMING Kp,Ki,Kd = (50, 0, 0) front dist = 165, avoidance dist = 0, gl_bChgConnect = 0, anomaly code = IR_BEAM_AVAILABLE Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd FrontD RearD Charger Connected at 52402 38.2: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.49 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 38.7: gl_Left/RightCenterCm = 62.9/17.9, Left/RightSteerVal = -1.00/0.01 Robot has successfully connected to charger! 52837: Reset VL53L0X Teensy to reduce current consumption ChgSec BattV TotalI RunI ChgI bChging 0.0 7.4632 1.2968 0.2645 1.0323 TRUE 0.1 7.4794 1.2935 0.2355 1.0581 TRUE Charging timout value of 0.20 minutes expired at Restart VL53L0X Teensy... 64851: got 255 from VL53L0X Teensy Teensy setup() finished at 64953 mSec VL53L0X Teensy Ready at 64954 Just after WaitForVL53L0XTeensy() and UpdateAllDistances() LeftD RightD FrontD RearD 400.0 400.0 4 400.0 in ExecDisconManeuver() with BattV = 7.4697 In ExecDisconManeuver() with left/right distances = 400.0/400.0 MTFD: at start, tgt = 70cm, curr_dist = 3, front/rear var = 83325/13332 MTFD: Stopped with front dist = 71, anomaly code = IR_BEAM_AVAILABLE 71916: Initializing IR Beam Total Value Averaging Array...Done Just before call to HandleAnomalousConditions() with anomaly code = NONE In HandleAnomalousConditions(RIGHT) with last anomaly code = NONE In HandleAnomalousConditions(RIGHT) ANOMALY_NONE CASE 57.8: gl_Left/RightCenterCm = 32.7/145.5, Left/RightSteerVal = 0.00/0.09 gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset() TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 57.8 Just after CaptureWallOffset(TRACKING_LEFT, 32.7) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 57.9 33.0 142.0 -165.1 33.1 31.9 0.16 225 151 115756 7346 6 127 NONE LEFT 58.0 33.0 144.8 -169.1 32.8 32.5 0.06 190 322 99779 7329 32 117 NONE LEFT 58.1 32.5 147.4 -175.8 32.8 32.3 0.05 175 124 80283 7881 43 106 NONE LEFT 58.2 32.7 147.6 178.5 32.0 33.0 -0.10 132 120 56728 7922 101 49 NONE LEFT 58.3 33.5 149.1 176.8 32.1 33.0 -0.09 131 184 28594 8135 83 66 NONE LEFT 58.4 33.5 149.3 177.0 31.5 32.2 -0.07 131 225 4229 8457 79 70 NONE LEFT 58.5 33.0 149.5 177.9 31.5 32.4 -0.09 126 360 4196 8675 86 63 NONE LEFT 58.6 32.6 149.5 179.2 31.6 31.8 -0.02 121 189 4091 8923 68 81 NONE LEFT 58.7 32.8 149.9 179.1 32.9 32.5 0.00 116 170 3935 9481 59 90 NONE LEFT 58.8 33.0 149.9 178.0 32.6 33.5 -0.09 115 134 3757 9753 82 67 NONE LEFT 58.9 33.3 148.7 176.3 32.5 33.3 -0.08 125 175 3577 10066 83 67 NONE LEFT 59.0 32.9 147.1 177.2 31.4 33.1 -0.17 110 147 3367 10520 127 22 NONE LEFT 59.1 32.6 146.3 -179.7 31.6 32.6 -0.10 99 154 3138 11015 104 45 NONE LEFT 59.2 32.1 147.4 -175.8 30.9 31.9 -0.10 100 133 2913 11341 102 47 NONE LEFT 59.3 32.0 148.5 -173.6 31.7 31.7 0.00 113 100 2693 11880 61 88 NONE LEFT 59.4 32.5 147.2 -173.5 32.0 31.9 0.01 113 232 2478 11945 58 91 NONE LEFT 59.5 33.1 144.9 -174.4 32.6 32.1 -0.01 100 60 2264 12942 57 92 NONE LEFT 59.6 33.6 144.3 -175.3 32.8 33.0 -0.02 90 204 2073 13157 61 88 NONE LEFT 59.7 33.5 142.8 -176.0 33.1 33.2 -0.01 83 184 1905 13393 57 92 NONE LEFT 59.8 33.3 142.6 -177.0 33.1 33.3 -0.02 78 160 1771 13706 61 88 NONE LEFT 59.9 33.9 137.8 -177.9 33.3 34.0 -0.07 73 190 1676 13922 75 74 NONE LEFT 60.0 33.8 127.1 -179.3 33.3 34.4 -0.11 66 183 1631 14243 96 53 NONE LEFT 60.1 33.9 113.0 -179.4 33.5 34.0 -0.05 58 332 1640 14438 71 78 NONE LEFT 60.2 34.1 109.3 -179.2 33.6 33.6 -0.01 54 202 1692 14633 51 98 NONE LEFT 60.3 34.5 108.3 -179.2 33.5 34.2 -0.07 51 199 1713 14654 70 79 NONE LEFT 60.4 34.4 112.3 -179.8 33.5 34.5 -0.10 43 244 1340 14590 87 62 NONE LEFT 60.5 34.4 130.6 179.6 33.6 33.9 -0.03 37 229 1120 14605 57 92 NONE LEFT 60.6 34.5 145.5 179.5 34.1 34.9 -0.08 33 251 984 14621 75 74 NONE LEFT 60.7 35.0 150.9 179.4 35.1 34.0 0.11 29 274 1057 14475 8 127 WALL_OFFSET_DIST_AHEAD LEFT Stopping Motors! Anomaly Code is 4 --> WALL_OFFSET_DIST_AHEAD 60.8: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.35 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 61.2: gl_Left/RightCenterCm = 294.0/146.6, Left/RightSteerVal = 1.00/-0.57 Just before call to HandleAnomalousConditions() with anomaly code = WALL_OFFSET_DIST_AHEAD In HandleAnomalousConditions(LEFT) with last anomaly code = WALL_OFFSET_DIST_AHEAD ANOMALY_WALL_OFFSET_DIST_AHEAD case detected with tracking case LEFT In BackupAndTurn90Deg(CW)with front distance = 8 digitalWrite(CHG_FIN_LED_PIN, LOW); MTFD: at start, tgt = 30cm, curr_dist = 9, front/rear var = 83325/13332 |