Posted 02 September 2023
As described in this earlier post, I hatched a plan to use ‘out of range’ (>= 1) steering values to manage ‘open doorway’, ‘open corner’ and other navigation anomalies, but as usual, this has turned out to be ‘easier said than done’. The problem of deciding what to do after an excessive steerval anomaly detection is complicated by the robot’s behavior leading up to the anomaly. As the steering value rises toward the limit, the robot naturally changes its wheel motor speeds in a manner intended to bring the steering value back down again. In the case of an ‘open doorway’ event, this means the robot turns into the open doorway instead of continuing straight. Then, when the robot stops in response to the associated anomaly detection, it is oriented in a way that makes figuring out what to do next very difficult. The following short video, illustrates the problem. In the first part of the video the robot stops at the 45º break and executes the current anomaly response algorithm; it turns parallel to the left wall, turns 90º to the wall and goes forward to the desired offset distance, then makes another 90º turn and continues wall-following. At the end of the video at the next ‘excess steerval’ anomaly, the robot makes an abrupt turn into the open doorway and can’t recover.
So, what to do, what to do? The robot doesn’t have any idea where it is in the house; all it knows is the steering value for the tracked wall first increased sharply (causing the abrupt left turn shown in the video) and then went out of range (causing the robot to stop). What I want the robot to do is figure out that there is a trackable wall opposite the open doorway, and discovering/tracking it is the preferred behavior.
So, how does the robot ‘discover’ that the other wall is a better tracking candidate? The following diagram shows the situation just after the robot stops.
From the robot’s point of view, the nearest wall is still on its left side but the short left-side segment (the side of a bookcase in this particular configuration) isn’t long enough to actually perform a ‘RotateToParallelOrientation’ procedure. In the meantime, the perfectly good wall a meter or so away is totally ignored. Oops! May have found the problem; if the center distance to the other wall is more than 1m – it is outside the current value for ‘MAX_TRACKING_DISTANCE_CM’
07 September 2023 Update:
After further review, it appears the issue wasn’t so much distances > MAX_TRACKING_DISTANCE_CM as having the reported distance to both sides < MAX_TRACKING_DISTANCE_CM, even after the ‘excessive steerval’ anomaly trigger. This occurs, for instance, when the robot successfully navigates the ‘open door’ into my office, transitions to the right-hand wall (kitchen counter) and then runs off the end of the counter wall. This generates an ‘excessive steerval’ anomaly, but when the robot checks again to decide what to do, both side sensors report distance values < MAX_TRACKING_DISTANCE_CM, most likely due to a ‘false’ distance report from a kitchen eat-in counter bar stool, as shown in the following diagram.
In the above diagram, the robot is located at ‘Position B’. There is nothing to track on to its right (except the legs and cross-braces of the bar stool), and a nice wall to the left. However, because the robot can still ‘see’ the bar stool legs on the right, both sides report distances < MAX_TRACKING_DISTANCE_CM.
So, I’m thinking about introducing the ability for the robot to discern the ‘best’ way forward, but this begs the question of what ‘forward’ and ‘best’ mean in this context.
Forward:
I propose that ‘forward’ in this context means – the direction roughly in line with the previous average heading of the robot. In the above diagram, the average robot heading was toward the top of the diagram, so ‘forward’ would mean no more than 90º either side of this heading (note that WallE3 knows nothing about compass headings – so the baseline average heading at the point of decision can be anything).
Best:
I propose that ‘best’ in this context means “the wall that subtends the largest angle in the ‘forward’ (i.e. +/- 90º from average heading from last 2-3 sec before the anomaly) hemisphere.
In order to implement this scheme, I need to also implement a way of determining the average heading value for the last 2-3 sec. This implies an array of heading values that is updated at each measurement interval, with a length that covers the desired time interval. I already do this with front and side distances with aFrontDist, aLeftDistCM, aRightDistCM, and aRearDistCM, with various sizes. The aFrontDist and aRearDistCM sizes are supposed to be large enough so that the front and rear variance calculations will trigger a ‘stuck’ anomaly after about 5 sec of no movement; however, I see from the current code that while the front array size was bumped to 100 just last month to accommodate the new 50mSec cycle time, the rear one is still at 50. It occurs to me that the array size should automatically change based on the cycle time to maintain the desired ‘stuck’ detection time frame (nominally 5 sec). To do this I could create a ‘STUCK_TIME_FRAME_SEC’ constant, and then multiply this by the measurement time interval in Sec in the aFront and aRear array declarations. Here’s what I came up with:
1 2 3 4 5 6 7 8 9 |
//09/09/23 Experiment with compile-time adjustable array sizes const uint16_t STUCK_TIME_FRAME_SEC = 5; const uint16_t AVG_HDG_TIME_FRAME_SEC = 3; const uint16_t STUCK_ARRAY_SIZE = (uint16_t)1000 * STUCK_TIME_FRAME_SEC / MSEC_PER_DIST_UPDATE; const uint16_t AVG_HDG_ARRAY_SIZE = (uint16_t)1000 * AVG_HDG_TIME_FRAME_SEC / MSEC_PER_DIST_UPDATE; uint16_t aFrontDist[STUCK_ARRAY_SIZE]; float aRearDistCM[STUCK_ARRAY_SIZE]; float aAvgHdgDeg[AVG_HDG_ARRAY_SIZE]; |
This at least compiles, so I am optimistic that I haven’t created a huge disaster.
To start this implementation, I created a new global variable ‘gl_AvgHeadingDeg’, a new function to initialize a new ‘aAvgHdgDeg’ array, and ‘UpdateAvgHdgArray()’ to update the array and the average heading at each regular measurement interval as shown
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 |
//added 09/09/23 for 'choose better side' support float gl_AvgHeadingDeg; void InitAvgHdgArray() { //Purpose: Iniitialize the new 'average heading' array //Inputs: None //Outputs: aAvgHdgDeg initialized to all zeros float hdg = UpdateIMUHdgValDeg(); for (int i = 0; i < AVG_HDG_ARRAY_SIZE; i++) { aAvgHdgDeg[i] = hdg; } gl_AvgHeadingDeg = hdg; } float UpdateAvgHdgArray(float newheading) { //Purpose: Update average heading array //Provenance: 09/09/23 //Inputs: // newheading = latest heading from IMU //Outputs: // all array values shifted down one, and newheading added to top // new average heading //Notes: //DEBUG!! gl_pSerPort->printf("%lu: UpdateAvgHdgArray(%2.1f) before array update\n", millis(), newheading); for (int i = 0; i < AVG_HDG_ARRAY_SIZE - 1; i++) { gl_pSerPort->printf("aAvgHdg[%d] = %d\n", i, aAvgHdgDeg[i]) ; delay(10); } //DEBUG!! for (int i = 0; i < AVG_HDG_ARRAY_SIZE - 1; i++) { aAvgHdgDeg[i] = aAvgHdgDeg[i + 1]; } aAvgHdgDeg[AVG_HDG_ARRAY_SIZE - 1] = newheading; //DEBUG!! gl_pSerPort->printf("%lu: UpdateFrontDistanceArray(%d) after array update\n",millis(), newheading); for (int i = 0; i < AVG_HDG_ARRAY_SIZE; i++) { gl_pSerPort->printf("aAvgHdgDeg[%d] = %d\n", i, aAvgHdgDeg[i]) ; delay(10); } //DEBUG!! //compute new average float avg = 0; for (int i = 0; i < AVG_HDG_ARRAY_SIZE; i++) { avg+= aAvgHdgDeg[i]; } avg = avg / AVG_HDG_ARRAY_SIZE; return avg; } |
Then I added code to UpdateAllEnvironmentalParameters() to update the average heading array, as shown below:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
void UpdateAllEnvironmentParameters(WallTrackingCases trkdir) { //... digitalWrite(DURATION_MEASUREMENT_PIN1, HIGH);//start measurement pulse //Step1: Update all left/right/rear/front distances UpdateAllDistances(); UpdateIMUHdgValDeg(); //added 09/03/23 gl_AvgHeadingDeg = UpdateAvgHdgArray(IMUHdgValDeg); //... } |
Then I created a ‘ChooseBetterTrackingSide()’ function to actually determine which side to ‘nominate’ for further wall tracking, 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 |
WallTrackingCases ChooseBetterTrackingSide() { //Purpose: Choose the better side for continued tracking. The 'better' side is the one that // has the most unbroken wall available in the 'forward' direction. See the 07/09/23 // update to https://www.fpaynter.com/2023/09/responding-to-steerval-out-of-range-anomalies/ //Inputs: // gl_CurTrackingCase = WallTrackingCases enum denoting the side WallE3 is currently tracking //Outputs: // A WallTrackingCases enum denoting the side to be tracked going forward //Plan: // Step1: Turn the robot to the average heading for the las 2-3 sec // Step2: Turn the robot 90deg CCW, in 10deg steps. At each step, get the distance to the // left wall using GetFrontDistCm, and the to the right wall using gl_RightCenterCm // Step3: Determine which side offers the longest gap-free wall distance //Notes: // 09/09/23 it is assumed that both left and right distances are < MAX_TRACKING_DIST_CM // at the start (othewise we wouldn't be here) //Step1: Turn the robot to the average heading from the last 2-3 sec float avghdg = UpdateAvgHdgArray(IMUHdgValDeg); //updates array and returns new avg TurnToHdgDeg(avghdg); //Step2: Turn the robot 90deg CCW, in 10deg steps. At each step, get the distance to the // left wall using GetFrontDistCm, and the to the right wall using gl_RightCenterCm uint8_t numleftwallsegments = 0; uint8_t numrightwallsegments = 0; for (size_t i = 0; i < 9; i++) { SpinTurn(true, 10); UpdateAllDistances(); float leftdist = (float)GetFrontDistCm(); //front dist returns integer float rightdist = gl_RightCenterCm; if (leftdist < MAX_TRACKING_DISTANCE_CM) { numleftwallsegments++; } if (rightdist < MAX_TRACKING_DISTANCE_CM) { numrightwallsegments++; } } //Step3: Determine which side offers the longest gap-free wall distance if (numleftwallsegments > numrightwallsegments) { gl_pSerPort->printf("ChooseBetterTrackingSide 'left' block with numleft/rightwallsegments = %d/%d\n", numleftwallsegments, numrightwallsegments); return TRACKING_LEFT; } else if (numrightwallsegments > numleftwallsegments) { gl_pSerPort->printf("ChooseBetterTrackingSide 'right' block with numleft/rightwallsegments = %d/%d\n", numleftwallsegments, numrightwallsegments); return TRACKING_RIGHT; } else { gl_pSerPort->printf("Stuck in ChooseBetterTrackingSide 'else' block with numleft/rightwallsegments = %d/%d: Quitting!\n", numleftwallsegments, numrightwallsegments); while (true) { CheckForUserInput(); delay(100); } } return TRACKING_NEITHER; } |
10 September 2023 Update:
To test the new average heading idea, I modified my existing ‘#ifdef HDG_ONLY’ code block to add the average heading, and then manually rotated the robot back and forth to see if the average heading value was reasonable. After the usual amount of code debugging, I got what I think are reasonable results, as shown in the following Excel plot:
With the actual and average headings stabilized, I very rapidly rotated the robot from ~0º to ~90º and then allowed the average heading value to catch up. As predicted with the current update rate of 50mSec, the average value array of 60 points took approximately 3Sec to catch up.
Now that I have the average heading value feature working, I can start testing the ability to actually use the average value. Here’s a short video and telemetry output from a test in my office showing the ‘ChooseBetterSide’ algorithm in action:
The salient points in the above video:
- At 4sec (3.3sec in telemetry), the robot detects an ‘EXCESS_STEER_VAL’ anomaly
- At about 4.5sec (3.8sec in telemetry) ChooseBetterSide() is called
- 4.5-10.0sec, the robot makes nine 10º CCW turns to measure left/right distances in ‘forward’ direction, and then turns back to the initial average heading.
- As shown in the telemetry, ChooseBetterSide found 7 left and 0 right trackable wall segments, and returned TRACKING_LEFT
- At 12sec, the robot executes a ‘RotateToParallelOrientation(TRACKING_LEFT) to prepare to track the left side wall
- At 13-15sec the robot tracks the left side wall. Note that because the robot was within the tracking distance window at the end of the RotateToParallelOrientation call, it started tracking immediately.
Here’s the telemetry from the above test 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 |
Sec LCen RCen Deg RF RR RStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.6 175.6 16.8 -0.0 17.1 16.4 0.07 476 97 67197 11769 5 127 NONE RIGHT 0.6 175.3 17.0 -0.5 17.6 16.6 0.10 469 97 63125 11359 18 127 NONE RIGHT 0.7 175.1 17.1 -2.2 17.2 16.7 0.05 459 97 59326 10998 2 127 NONE RIGHT 0.7 177.9 17.3 -5.2 17.2 16.6 0.06 446 97 55825 10692 4 127 NONE RIGHT 0.8 181.3 17.9 -8.4 17.5 16.8 0.07 428 97 52665 10436 8 127 NONE RIGHT 0.8 189.3 18.7 -13.8 20.1 17.5 0.13 394 96 49970 10227 34 115 NONE RIGHT 0.9 193.7 19.9 -18.6 19.0 18.0 0.10 375 96 47683 10068 33 116 NONE RIGHT 0.9 193.7 21.1 -22.0 20.8 19.1 0.17 188 95 47786 9954 62 87 NONE RIGHT 1.0 196.2 20.8 -25.6 22.6 19.1 0.17 158 84 48642 9934 57 92 NONE RIGHT 1.0 189.2 22.5 -28.5 24.5 19.9 0.46 140 68 50059 10038 127 16 NONE RIGHT 1.1 174.0 24.2 -30.0 26.3 21.9 0.44 128 64 51922 10207 127 5 NONE RIGHT 1.1 156.0 26.6 -29.5 28.0 23.2 0.48 123 63 54090 10414 127 0 NONE RIGHT 1.2 162.1 26.7 -28.7 29.4 24.6 0.48 124 64 56434 10640 127 0 NONE RIGHT 1.2 175.7 27.8 -26.9 30.0 25.3 0.47 127 68 58898 10876 127 0 NONE RIGHT 1.3 187.2 28.6 -23.8 30.2 26.1 0.41 138 78 61328 11077 127 0 NONE RIGHT 1.3 192.8 29.0 -20.6 30.8 26.8 0.40 155 92 63615 11238 127 0 NONE RIGHT 1.4 192.3 28.9 -15.5 30.1 27.3 0.28 178 109 65667 11344 127 10 NONE RIGHT 1.5 187.0 28.6 -11.3 29.4 26.8 0.33 361 117 65879 11440 127 2 NONE RIGHT 1.5 183.5 28.7 -7.5 29.3 27.3 0.20 367 120 66153 11540 118 31 NONE RIGHT 1.5 162.7 28.7 -3.5 29.3 27.3 0.20 400 121 66314 11644 117 33 NONE RIGHT 1.6 149.8 28.4 1.9 29.6 28.2 0.04 431 122 66400 11751 77 72 NONE RIGHT 1.7 139.8 28.4 7.4 29.1 28.6 0.05 583 127 66381 11849 78 71 NONE RIGHT 1.7 138.4 30.2 10.1 29.8 30.5 -0.07 579 130 66387 11939 52 97 NONE RIGHT 1.8 139.3 29.8 10.7 29.7 30.8 -0.11 364 137 66960 12008 24 125 NONE RIGHT 1.9 138.9 29.4 9.6 29.2 30.5 -0.13 370 138 67480 12066 15 127 NONE RIGHT 1.9 140.3 28.4 8.0 27.8 29.3 -0.15 569 140 67497 12109 2 127 NONE RIGHT 2.0 138.8 28.2 5.0 27.4 29.5 -0.21 586 144 67519 12130 0 127 NONE RIGHT 2.1 139.2 27.4 0.9 26.4 27.6 0.00 460 145 67526 12134 49 100 NONE RIGHT 2.1 140.6 27.2 -1.6 26.4 27.3 -0.09 410 146 67631 12113 24 125 NONE RIGHT 2.1 139.8 27.1 -4.0 27.1 27.2 -0.01 385 147 67740 12065 48 101 NONE RIGHT 2.2 140.6 26.6 -6.3 27.0 26.3 0.07 379 148 67724 11984 70 79 NONE RIGHT 2.2 143.0 27.2 -8.4 27.8 27.1 0.07 369 148 67574 11868 78 71 NONE RIGHT 2.3 142.5 27.4 -9.9 28.5 27.1 0.14 354 149 67285 11710 95 55 NONE RIGHT 2.3 142.5 28.2 -10.6 28.8 27.1 0.17 345 151 66785 11505 112 37 NONE RIGHT 2.3 143.8 28.2 -10.5 28.8 27.1 0.17 345 152 65983 11251 113 36 NONE RIGHT 2.4 143.6 27.8 -10.0 28.8 27.8 0.10 340 155 64872 10940 90 59 NONE RIGHT 2.4 142.6 28.5 -9.4 29.3 28.8 0.05 342 157 63375 10570 79 70 NONE RIGHT 2.5 140.8 28.6 -8.6 29.4 28.7 0.07 347 160 61444 10135 85 64 NONE RIGHT 2.5 140.5 29.1 -7.8 29.2 29.1 0.01 348 161 59064 9632 72 77 NONE RIGHT 2.5 140.5 29.2 -7.3 30.0 29.5 0.05 354 161 56181 9055 84 65 NONE RIGHT 2.6 139.1 29.7 -7.3 30.0 29.6 0.04 352 164 52787 8400 82 67 NONE RIGHT 2.6 137.3 29.6 -7.1 30.4 29.5 0.09 351 166 48836 7662 98 51 NONE RIGHT 2.7 137.2 30.1 -7.1 30.5 30.0 0.05 346 171 44297 6837 92 57 NONE RIGHT 2.7 137.2 30.4 -6.9 31.3 30.4 0.09 343 173 39117 5921 105 44 NONE RIGHT 2.8 138.1 30.6 -6.6 31.0 30.6 0.04 346 176 33238 4907 90 60 NONE RIGHT 2.8 137.5 30.7 -5.9 31.3 30.7 0.06 344 178 26627 3793 95 54 NONE RIGHT 2.9 136.0 30.8 -5.4 31.4 31.5 -0.01 341 180 19235 2571 72 77 NONE RIGHT 2.9 130.1 30.9 -4.9 31.6 31.8 -0.02 348 182 15055 1236 68 81 NONE RIGHT 3.0 74.9 31.3 -4.7 31.6 31.5 0.01 339 184 14732 1284 84 65 NONE RIGHT 3.0 57.2 30.7 -4.7 31.8 32.4 -0.06 340 187 14399 1328 55 94 NONE RIGHT 3.1 52.7 31.0 -4.2 31.4 31.9 -0.05 338 189 14059 1374 63 86 NONE RIGHT 3.1 50.9 31.7 -4.5 32.9 33.2 -0.16 338 192 13741 1417 21 127 NONE RIGHT 3.2 49.9 31.6 -5.2 35.1 32.3 0.06 332 194 13461 1458 98 51 NONE RIGHT 3.2 49.3 33.1 -6.3 116.6 32.5 0.26 325 197 13229 1497 127 0 NONE RIGHT RightSteer = 1.0, result = TRUE 3.3 49.6 190.1 -6.9 181.9 32.1 1.00 326 199 13060 1535 127 0 EXCESS_STEER_VAL RIGHT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 3.3: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.61 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 3.7: gl_Left/RightCenterCm = 47.7/188.4, Left/RightSteerVal = -0.05/0.05 In HandleAnomalousConditions(RIGHT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(RIGHT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase() 3877: gl_Left/RightCenterCm = 46.5/189.1, Left/RightSteerVal = -0.10/0.40 3878: gl_Left/RightCenterCm = 46.5/189.1, Left/RightSteerVal = -0.10/0.40 Calling ChooseBetterTrackingSide() In ChooseBetterTrackingSide with avg hdg = -5.2 TurnToHdgDeg: currHdg = -1.9, tgtHdg = -5.2, retries = 0 SpinTurn: Init hdg = -1.95 deg, Turn = 3.27 deg, tgt = -5.22 deg, timeout = 1.00 sec SpinTurn: Init hdg = -4.19 deg, Turn = 10.00 deg, tgt = -14.19 deg, timeout = 1.00 sec SpinTurn: Init hdg = -15.06 deg, Turn = 10.00 deg, tgt = -25.06 deg, timeout = 1.00 sec SpinTurn: Init hdg = -25.34 deg, Turn = 10.00 deg, tgt = -35.34 deg, timeout = 1.00 sec SpinTurn: Init hdg = -35.35 deg, Turn = 10.00 deg, tgt = -45.35 deg, timeout = 1.00 sec SpinTurn: Init hdg = -45.35 deg, Turn = 10.00 deg, tgt = -55.35 deg, timeout = 1.00 sec SpinTurn: Init hdg = -55.31 deg, Turn = 10.00 deg, tgt = -65.31 deg, timeout = 1.00 sec SpinTurn: Init hdg = -65.82 deg, Turn = 10.00 deg, tgt = -75.82 deg, timeout = 1.00 sec SpinTurn: Init hdg = -76.17 deg, Turn = 10.00 deg, tgt = -86.17 deg, timeout = 1.00 sec SpinTurn: Init hdg = -86.72 deg, Turn = 10.00 deg, tgt = -96.72 deg, timeout = 1.00 sec TurnToHdgDeg: currHdg = -97.4, tgtHdg = -5.2, retries = 0 SpinTurn: Init hdg = -97.61 deg, Turn = 92.16 deg, tgt = -5.45 deg, timeout = 4.10 sec ChooseBetterTrackingSide 'left' block with numleft/rightwallsegments = 7/0 ChooseBetterTrackingSide() returned LEFT In RotateToParallelOrientation(LEFT) SpinTurn: Init hdg = -5.00 deg, Turn = 8.34 deg, tgt = 3.34 deg, timeout = 1.11 sec TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 13.7 Just after CaptureWallOffset(TRACKING_LEFT, 43.7) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 13.7 42.6 188.2 2.0 41.6 43.4 -0.17 342 224 17240 1681 65 84 NONE LEFT 13.8 42.8 187.2 2.2 41.1 43.0 -0.19 344 224 17015 1753 71 78 NONE LEFT 13.8 43.7 186.8 2.4 41.9 42.9 -0.10 339 223 16990 1780 28 121 NONE LEFT 13.9 42.6 188.1 2.1 41.6 42.5 -0.09 339 224 16922 1749 29 120 NONE LEFT 13.9 44.0 187.3 0.6 41.6 43.3 -0.17 334 226 16754 1685 49 100 NONE LEFT 14.0 42.8 189.7 -1.5 41.5 42.8 -0.13 323 227 15954 1597 43 106 NONE LEFT 14.1 44.2 191.0 -4.0 41.6 43.5 -0.19 303 229 14568 1499 56 93 NONE LEFT 14.1 43.0 199.4 -5.3 42.0 44.0 -0.20 294 230 13841 1395 74 76 NONE LEFT 14.1 43.1 202.3 -6.4 40.8 43.7 -0.29 290 230 13784 1315 117 32 NONE LEFT 14.2 43.0 202.0 -6.9 41.5 43.8 -0.23 285 232 13073 1266 87 62 NONE LEFT 14.2 42.7 201.2 -6.3 41.1 42.8 -0.17 287 234 11569 1260 65 84 NONE LEFT 14.3 42.5 203.9 -6.0 40.8 42.5 -0.17 285 237 10477 1264 64 85 NONE LEFT 14.3 43.4 204.1 -6.0 40.6 41.8 -0.12 277 238 10071 1268 34 115 NONE LEFT 14.4 41.6 203.8 -6.4 40.0 42.4 -0.24 275 241 9845 1273 105 44 NONE LEFT 14.4 42.2 206.8 -6.7 39.5 41.4 -0.19 277 242 9673 1271 73 76 NONE LEFT 14.5 40.8 209.7 -6.4 39.4 41.8 -0.24 271 245 9525 1274 114 35 NONE LEFT 14.5 41.7 210.2 -6.3 38.9 41.4 -0.24 267 247 9411 1278 110 39 NONE LEFT 14.6 41.2 210.1 -5.6 38.0 40.8 -0.19 267 248 9327 1289 80 69 NONE LEFT 14.6 40.4 209.7 -4.7 37.7 40.2 -0.25 265 250 9251 1299 118 31 NONE LEFT 14.7 40.0 210.7 -3.9 37.7 39.6 -0.19 264 257 9176 1318 87 62 NONE LEFT 14.7 39.2 210.7 -3.2 36.9 38.9 -0.20 268 258 9099 1339 95 54 NONE LEFT 14.8 39.7 211.0 -2.3 37.1 38.9 -0.18 274 260 9007 1355 89 60 NONE LEFT 14.8 39.4 208.8 -1.7 37.1 38.5 -0.14 271 264 8902 1369 69 80 NONE LEFT 14.9 39.1 209.3 -1.2 36.9 38.7 -0.18 273 265 8783 1377 87 62 NONE LEFT 14.9 38.8 210.2 -1.2 36.6 38.3 -0.17 268 267 8652 1384 92 57 NONE LEFT 15.0 38.7 209.7 -1.0 36.5 38.0 -0.15 269 269 8520 1381 79 70 NONE LEFT 15.0 38.8 210.1 -0.6 36.2 37.7 -0.15 267 272 8392 1378 78 71 NONE LEFT 15.1 38.9 209.3 0.0 36.7 38.1 -0.14 264 274 8272 1376 75 74 NONE LEFT 15.1 38.6 210.6 0.3 36.5 38.0 -0.15 261 275 8148 1367 78 71 NONE LEFT 15.2 38.4 210.1 -0.0 36.4 37.8 -0.14 260 278 8016 1360 75 74 NONE LEFT 15.2 38.5 210.9 0.2 36.6 37.9 -0.13 262 280 7886 1351 71 78 NONE LEFT 15.3 38.2 210.8 0.3 36.4 37.8 -0.18 260 281 7745 1337 94 55 NONE LEFT 15.3 38.1 209.2 0.4 36.1 37.7 -0.13 256 283 7600 1321 72 77 NONE LEFT 15.4 37.8 209.6 0.8 36.4 37.2 -0.08 257 286 7464 1300 55 95 NONE LEFT 15.4 38.0 209.7 0.9 36.2 37.1 -0.09 253 291 7324 1284 50 99 NONE LEFT 15.5 38.3 210.0 0.9 37.1 37.0 0.01 255 290 7182 1260 17 127 NONE LEFT 15.5 38.0 208.7 0.6 39.3 37.2 0.21 250 294 7044 1247 0 127 NONE LEFT 15.6 39.4 208.3 -0.8 46.2 37.4 0.88 241 298 6923 1234 0 127 NONE LEFT 15.6 41.6 209.0 -2.8 73.1 38.5 1.00 231 300 6810 1218 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL |
Next I ran a ‘field’ test in the hallway outside my office, as shown in the following short video:
Interestingly, the normal ‘open door’ algorithm was used for the open doorway into my office, at about 14sec into the video, causing WallE3 to transition to the ‘other’ (kitchen counter) wall. At the end of the kitchen counter (at about 31sec) the ‘ChooseBestSide’ algorithm was used to transition back to the left side wall. Unfortunately he robot failed to make the left turn into the bedroom hallway at the end of the run. More work to go!
Here’s the telemetry from 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 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 |
Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 0.5: gl_Left/RightCenterCm = 23.3/105.2, Left/RightSteerVal = -0.19/-0.93 In HandleAnomalousConditions(NEITHER) with last anomaly code = NONE In HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE- 0.5: gl_Left/RightCenterCm = 23.0/104.6, Left/RightSteerVal = -0.11/-0.89 gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset() TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 0.6 Just after CaptureWallOffset(TRACKING_LEFT, 23.0) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.7 23.9 105.1 0.1 23.3 23.9 -0.06 474 22 67232 13021 127 9 NONE LEFT 0.8 23.6 105.6 2.0 23.1 24.6 -0.15 478 23 63144 13035 127 0 NONE LEFT 0.8 23.2 108.6 4.5 23.1 22.5 0.06 466 23 59325 13102 107 42 NONE LEFT 0.8 23.2 120.1 7.3 23.6 22.5 0.06 449 23 55812 13222 103 47 NONE LEFT 0.9 23.5 134.0 10.0 23.6 24.4 -0.08 431 24 52637 13382 127 0 NONE LEFT 0.9 24.2 134.0 12.2 24.5 24.6 -0.01 419 26 49788 13574 122 27 NONE LEFT 1.0 25.1 139.2 14.6 25.1 23.1 0.20 406 27 47281 13795 57 92 NONE LEFT 1.0 24.5 138.2 17.1 23.4 24.4 -0.10 394 29 45119 14042 127 3 NONE LEFT 1.1 25.4 130.2 19.5 24.5 24.4 0.01 377 33 43346 14305 109 40 NONE LEFT 1.1 26.0 114.9 21.6 26.1 25.4 0.07 354 35 42022 14588 80 69 NONE LEFT 1.1 26.8 108.5 24.1 27.0 26.6 0.04 342 37 41062 14881 88 61 NONE LEFT 1.2 28.7 105.9 25.8 28.5 28.4 0.01 350 41 40270 15177 85 64 NONE LEFT 1.2 30.0 105.9 26.2 30.2 28.7 0.15 350 44 39709 15477 32 117 NONE LEFT 1.3 31.4 105.0 26.7 31.5 29.7 0.18 345 46 39410 15785 19 127 NONE LEFT 1.4 32.3 104.9 25.7 33.9 31.0 0.29 344 48 39324 16096 0 127 NONE LEFT 1.4 33.5 113.8 24.3 35.2 31.9 0.33 340 50 39462 16414 0 127 NONE LEFT 1.4 34.7 134.1 21.7 36.0 33.6 0.24 335 52 39818 16723 0 127 NONE LEFT 1.5 35.1 140.6 19.5 36.6 33.5 0.31 342 53 40260 17030 0 127 NONE LEFT 1.6 35.2 132.1 15.4 35.5 34.0 0.22 368 54 40607 17336 0 127 NONE LEFT 1.6 35.4 104.8 12.2 35.3 34.4 0.09 378 54 40991 17636 7 127 NONE LEFT 1.7 35.1 92.2 8.8 34.0 34.3 -0.03 393 54 41362 17921 48 101 NONE LEFT 1.7 34.2 89.1 6.1 33.9 34.5 -0.06 398 55 41780 18190 67 83 NONE LEFT 1.8 34.4 87.5 3.3 33.4 33.6 -0.02 414 56 42157 18435 54 95 NONE LEFT 1.8 34.1 90.8 2.0 33.9 33.9 0.00 424 56 42519 18650 47 102 NONE LEFT 1.9 34.3 102.2 1.1 33.2 34.0 -0.08 427 58 42891 18827 73 76 NONE LEFT 1.9 34.4 116.8 0.3 33.1 34.3 -0.12 433 60 43237 18961 94 55 NONE LEFT 2.0 34.8 133.3 0.0 33.0 33.9 -0.09 426 61 43604 19050 79 70 NONE LEFT 2.0 34.6 139.4 0.7 33.2 34.4 -0.12 425 64 43936 19086 95 55 NONE LEFT 2.1 34.4 144.2 1.3 33.7 34.4 -0.07 425 66 44205 19065 72 77 NONE LEFT 2.1 34.3 144.6 1.6 34.1 34.2 -0.01 412 68 44452 18977 51 98 NONE LEFT 2.2 34.9 145.7 1.6 34.1 34.3 -0.02 416 70 44558 18823 53 96 NONE LEFT 2.2 35.4 146.4 1.1 34.1 34.8 -0.09 411 72 44543 18596 70 79 NONE LEFT 2.3 35.4 147.8 0.4 34.6 35.3 -0.07 414 75 44337 18291 64 85 NONE LEFT 2.3 35.6 147.4 0.0 34.1 35.0 -0.09 410 79 43943 17898 71 78 NONE LEFT 2.4 35.6 147.7 -0.2 34.1 35.7 -0.16 410 82 43313 17417 101 48 NONE LEFT 2.4 35.9 147.4 -0.2 34.3 35.3 -0.10 413 84 42400 16843 83 66 NONE LEFT 2.5 35.4 147.4 0.7 34.1 35.8 -0.17 404 86 41223 16171 111 38 NONE LEFT 2.5 35.8 147.3 1.1 34.5 35.9 -0.14 403 89 39711 15396 97 53 NONE LEFT 2.6 35.7 147.1 2.0 34.6 35.3 -0.07 400 91 37834 14512 66 83 NONE LEFT 2.6 36.2 147.3 2.8 34.9 36.5 -0.16 393 94 35570 13515 94 55 NONE LEFT 2.7 35.9 147.1 2.9 35.6 36.2 -0.06 385 97 32878 12398 63 87 NONE LEFT 2.7 36.4 147.6 2.9 35.6 35.8 -0.02 380 100 29703 11157 41 108 NONE LEFT 2.8 36.6 147.7 2.5 36.4 35.9 0.05 376 102 25997 9785 16 127 NONE LEFT 2.8 36.8 147.2 1.4 36.3 37.0 -0.07 379 105 21697 8278 55 94 NONE LEFT 2.9 36.8 146.0 -0.0 35.9 36.7 -0.08 381 106 16768 6629 60 89 NONE LEFT 2.9 36.9 145.5 -1.7 36.5 37.1 -0.06 395 111 11155 4835 54 95 NONE LEFT 3.0 37.0 146.0 -3.0 36.0 37.2 -0.12 386 112 4851 2887 73 76 NONE LEFT 3.1 37.1 145.2 -3.6 35.9 36.7 -0.08 1000 116 8411 784 55 94 NONE LEFT 3.2 36.9 145.6 -4.3 35.7 37.3 -0.16 975 121 14570 823 93 56 NONE LEFT 3.3 37.5 146.3 -4.5 35.6 37.1 -0.15 1000 126 21074 865 86 63 NONE LEFT 3.4 36.7 146.5 -4.0 35.5 37.0 -0.15 1000 131 27360 912 92 57 NONE LEFT 3.4 36.3 146.1 -3.2 34.8 36.4 -0.16 1000 136 33420 961 95 54 NONE LEFT 3.5 36.2 146.6 -2.3 34.6 36.4 -0.18 846 142 36433 1014 109 40 NONE LEFT 3.6 36.5 146.0 -0.9 35.0 36.6 -0.16 354 145 36658 1063 96 53 NONE LEFT 3.7 35.9 146.2 0.1 35.0 36.0 -0.10 354 147 36863 1108 82 67 NONE LEFT 3.7 36.5 146.0 0.8 35.3 36.9 -0.16 349 150 37068 1154 95 54 NONE LEFT 3.7 36.9 146.0 1.3 35.3 36.7 -0.14 339 153 37292 1200 89 60 NONE LEFT 3.8 36.2 146.1 1.8 35.6 36.3 -0.07 340 155 37482 1244 59 90 NONE LEFT 3.8 36.6 145.2 2.3 35.9 37.2 -0.13 333 157 37656 1287 83 66 NONE LEFT 3.9 36.6 145.5 2.3 36.1 37.5 -0.14 330 159 37763 1331 88 61 NONE LEFT 3.9 37.3 146.1 2.0 36.1 37.5 -0.14 332 159 37809 1369 82 67 NONE LEFT 3.9 37.3 145.0 2.1 36.1 37.4 -0.13 327 161 37914 1407 78 71 NONE LEFT 4.0 36.9 145.0 2.0 36.3 37.8 -0.15 326 164 38023 1447 91 58 NONE LEFT 4.0 36.9 144.6 2.1 36.8 36.9 -0.01 327 166 38106 1486 39 110 NONE LEFT 4.0 37.6 144.0 2.2 36.5 36.6 -0.01 325 168 38195 1525 29 120 NONE LEFT 4.1 37.4 143.2 1.8 36.6 37.2 -0.06 315 170 38319 1560 46 104 NONE LEFT 4.1 37.4 143.4 1.2 37.0 37.2 -0.06 324 170 38372 1591 47 103 NONE LEFT 4.2 37.7 143.1 0.4 37.0 37.9 -0.09 323 173 38461 1620 56 93 NONE LEFT 4.2 38.1 143.1 -0.4 37.0 38.3 -0.13 326 176 38633 1642 70 79 NONE LEFT 4.2 37.8 141.3 -1.0 36.8 38.1 -0.13 323 178 38849 1660 78 71 NONE LEFT 4.3 38.0 138.3 -1.6 36.9 38.0 -0.11 319 181 39119 1676 65 84 NONE LEFT 4.4 37.9 135.1 -1.9 36.6 38.2 -0.16 324 183 39370 1682 87 62 NONE LEFT 4.4 37.9 135.6 -2.0 36.8 38.3 -0.15 317 187 39675 1690 85 64 NONE LEFT 4.5 37.9 136.9 -1.7 37.0 38.2 -0.11 309 189 40022 1691 72 77 NONE LEFT 4.6 38.3 137.6 -1.2 37.1 38.6 -0.15 300 194 40409 1695 78 71 NONE LEFT 4.6 38.1 136.8 -1.0 36.8 38.9 -0.21 300 197 40784 1697 111 38 NONE LEFT 4.7 38.4 137.9 -0.8 36.9 38.7 -0.18 297 201 41159 1698 97 52 NONE LEFT 4.7 37.8 138.3 0.2 37.2 38.7 -0.15 292 204 41549 1698 86 63 NONE LEFT 4.7 38.2 138.4 0.8 37.5 38.3 -0.08 297 206 41898 1693 49 100 NONE LEFT 4.8 38.2 137.7 1.1 37.5 38.3 -0.08 288 206 42277 1681 47 103 NONE LEFT 4.8 38.3 137.7 1.2 37.4 38.1 -0.07 285 205 42663 1657 43 106 NONE LEFT 4.8 38.4 137.3 1.1 37.5 38.0 -0.05 283 206 43044 1630 36 113 NONE LEFT 4.9 38.6 136.8 0.6 37.5 38.2 -0.07 284 208 43407 1600 43 106 NONE LEFT 4.9 38.4 138.5 0.2 37.8 38.2 -0.07 279 208 43783 1569 43 106 NONE LEFT 5.0 38.4 138.5 -0.9 37.8 38.5 -0.07 288 209 44099 1534 43 106 NONE LEFT 5.0 38.7 139.0 -2.4 37.3 38.9 -0.16 289 214 44398 1503 86 63 NONE LEFT 5.1 38.9 138.4 -4.1 37.8 38.8 -0.10 828 220 47819 1478 62 87 NONE LEFT 5.2 39.0 139.3 -3.9 38.0 39.2 -0.12 1000 225 54435 1460 61 89 NONE LEFT 5.3 38.9 140.8 -3.4 37.4 39.0 -0.16 1000 228 60760 1444 87 62 NONE LEFT 5.4 38.1 141.1 -3.0 36.9 39.2 -0.23 1000 235 66781 1439 118 31 NONE LEFT 5.5 38.4 141.4 -1.6 36.8 38.0 -0.12 786 240 68817 1435 71 79 NONE LEFT 5.5 38.5 141.9 -0.7 36.7 38.0 -0.13 265 241 69456 1427 71 78 NONE LEFT 5.5 38.0 141.9 -0.6 36.9 38.3 -0.14 255 242 70151 1414 74 75 NONE LEFT 5.6 38.0 142.2 -0.4 37.1 38.3 -0.14 257 242 70828 1392 75 75 NONE LEFT 5.6 38.1 143.4 -0.4 37.1 38.3 -0.12 249 243 71562 1363 68 81 NONE LEFT 5.7 38.6 143.4 -0.5 37.4 38.2 -0.08 251 246 72302 1339 48 101 NONE LEFT 5.7 38.2 143.4 -0.5 37.1 38.5 -0.14 249 247 73020 1311 73 76 NONE LEFT 5.7 38.0 144.0 -0.5 37.3 38.5 -0.12 243 250 67512 1288 68 81 NONE LEFT 5.8 38.3 144.1 -0.3 37.1 38.0 -0.09 241 254 62131 1271 51 98 NONE LEFT 5.8 38.3 144.0 -0.2 37.1 38.0 -0.09 235 254 55772 1256 50 99 NONE LEFT 5.8 38.4 144.0 -0.4 37.1 39.0 -0.19 235 255 48946 1243 96 53 NONE LEFT 5.9 38.6 143.9 -0.7 37.4 38.2 -0.08 233 256 41662 1235 50 100 NONE LEFT 5.9 38.8 144.2 -0.7 37.6 38.2 -0.06 229 259 37411 1233 40 109 NONE LEFT 6.0 38.5 143.8 -1.2 37.2 38.5 -0.13 234 260 37717 1232 69 80 NONE LEFT 6.0 38.9 143.6 -1.6 37.9 38.6 -0.07 226 266 38052 1236 45 104 NONE LEFT 6.1 38.4 143.8 -2.1 37.6 38.4 -0.08 220 267 38405 1241 46 103 NONE LEFT 6.2 38.8 144.0 -3.7 37.5 38.4 -0.09 1000 274 46637 1256 50 99 NONE LEFT 6.2 38.3 143.9 -5.4 37.1 38.6 -0.15 964 280 53651 1279 76 73 NONE LEFT 6.3 38.6 144.7 -6.2 37.5 39.2 -0.17 974 287 60570 1317 91 58 NONE LEFT 6.4 38.0 145.3 -5.9 36.5 38.5 -0.20 655 289 61859 1353 102 47 NONE LEFT 6.5 37.8 145.6 -4.6 36.4 38.4 -0.20 652 293 63040 1387 109 40 NONE LEFT 6.6 38.1 145.0 -3.0 36.0 37.9 -0.21 812 296 66177 1415 113 36 NONE LEFT 6.7 37.0 144.8 -0.7 36.4 37.2 -0.08 719 298 67841 1445 57 92 NONE LEFT 6.7 37.2 145.5 0.5 36.2 37.8 -0.16 194 296 68686 1459 87 63 NONE LEFT 6.8 37.2 145.5 0.9 35.9 37.1 -0.12 189 295 69553 1466 75 74 NONE LEFT 6.8 37.2 145.7 1.1 35.9 37.1 -0.12 187 295 70387 1464 75 75 NONE LEFT 6.8 37.5 145.7 1.2 36.3 37.7 -0.14 186 294 71249 1446 81 68 NONE LEFT 6.8 38.1 145.4 1.2 36.9 37.6 -0.07 181 288 72138 1413 45 104 NONE LEFT 6.9 38.1 144.7 1.4 36.9 37.6 -0.07 181 288 73020 1375 43 106 NONE LEFT 6.9 38.1 144.7 1.6 36.8 38.3 -0.15 180 281 73884 1321 76 73 NONE LEFT 6.9 37.8 145.0 1.4 37.4 38.0 -0.06 180 281 74720 1267 48 101 NONE LEFT 7.0 37.8 145.0 1.2 37.4 38.0 -0.06 177 292 75581 1227 47 103 NONE LEFT 7.0 38.7 144.9 0.9 37.8 37.8 0.00 177 290 76405 1187 20 127 NONE LEFT 7.0 38.3 145.4 0.3 37.9 37.8 0.00 175 290 77206 1143 19 127 NONE LEFT 7.1 38.3 145.4 -0.7 37.9 38.1 -0.02 175 292 77962 1102 25 124 NONE LEFT 7.1 38.5 144.6 -2.5 36.6 38.3 -0.05 175 300 78706 1076 35 114 NONE LEFT 7.2 38.9 120.7 -6.3 36.6 38.4 -0.18 1000 319 85917 1087 92 57 NONE LEFT 7.3 38.8 118.1 -8.9 37.3 38.6 -0.13 468 336 85770 1140 72 77 NONE LEFT 7.4 38.7 121.3 -9.0 37.3 38.9 -0.16 326 329 85664 1166 88 62 NONE LEFT 7.4 38.3 123.7 -8.6 36.3 38.1 -0.18 335 329 85491 1181 95 54 NONE LEFT 7.5 38.1 125.6 -7.8 35.5 38.5 -0.30 326 340 85329 1206 127 0 NONE LEFT 7.6 37.6 127.9 -5.8 35.6 37.4 -0.18 450 340 85061 1226 106 43 NONE LEFT 7.7 36.2 126.1 -2.5 34.6 36.7 -0.21 1000 334 91550 1218 119 30 NONE LEFT 7.7 36.5 128.5 0.4 34.6 36.9 -0.19 681 334 92408 1201 113 36 NONE LEFT 7.8 36.0 128.5 1.5 34.6 36.3 -0.17 135 294 93740 1115 106 43 NONE LEFT 7.8 36.4 129.1 2.8 35.3 36.0 -0.07 134 252 95070 1044 60 89 NONE LEFT 7.8 36.4 126.4 4.0 35.3 36.0 -0.07 132 252 93330 988 57 92 NONE LEFT 7.9 36.2 126.4 4.8 35.0 36.7 -0.17 128 175 87674 1146 103 46 NONE LEFT 7.9 36.7 118.1 5.2 35.7 37.4 -0.17 125 150 81440 1421 106 43 NONE LEFT 7.9 36.7 118.1 5.4 35.7 37.4 -0.17 124 150 74602 1704 106 43 NONE LEFT 7.9 37.0 114.8 5.7 35.8 36.6 -0.08 122 144 71878 2016 57 93 NONE LEFT 8.0 36.9 116.3 6.2 36.6 36.8 -0.02 121 144 72745 2322 40 109 NONE LEFT 8.0 36.9 116.3 6.5 36.6 36.8 -0.02 118 141 73589 2643 40 110 NONE LEFT 8.0 37.1 114.9 6.5 36.6 37.6 -0.10 116 136 74442 2981 66 83 NONE LEFT 8.1 37.8 115.6 6.5 36.6 37.6 -0.10 115 136 75262 3311 68 82 NONE LEFT 8.1 37.8 115.6 6.5 36.6 37.8 -0.12 114 135 76084 3634 74 75 NONE LEFT 8.1 38.3 115.5 6.2 36.6 37.5 -0.09 110 142 76918 3914 51 98 NONE LEFT 8.2 38.3 119.9 5.9 37.2 37.5 -0.09 111 142 77709 4187 50 99 NONE LEFT 8.2 38.1 119.9 5.8 37.2 38.2 -0.10 110 142 78489 4451 60 89 NONE LEFT 8.2 38.6 124.0 5.6 38.0 38.2 -0.02 108 146 79251 4692 28 122 NONE LEFT 8.2 38.6 126.2 5.2 38.0 38.2 -0.02 107 146 80010 4924 26 124 NONE LEFT 8.3 38.1 126.2 4.6 39.2 38.5 0.07 105 151 80765 5121 0 127 NONE LEFT 8.3 38.8 124.0 3.7 41.3 38.9 0.24 105 165 81494 5262 0 127 NONE LEFT 8.3 38.8 124.0 2.7 41.3 38.9 0.24 106 165 82219 5395 0 127 NONE LEFT 8.4 39.0 123.6 1.6 43.7 38.8 0.49 106 183 82905 5465 0 127 NONE LEFT 8.4 40.4 120.9 0.3 45.4 39.5 0.59 106 183 83560 5529 0 127 NONE LEFT 8.5 40.5 106.4 -4.1 45.8 39.8 0.60 104 261 74406 5516 0 127 NONE LEFT 8.6 42.0 99.2 -10.4 45.4 41.2 0.42 577 358 66871 5755 0 127 NONE LEFT 8.6 42.4 92.7 -14.6 44.3 41.4 0.29 269 383 57039 6097 0 127 NONE LEFT 8.6 41.2 86.4 -17.3 42.9 41.8 0.11 248 319 53949 6164 0 127 NONE LEFT 8.7 40.9 81.3 -20.1 42.0 42.5 -0.05 238 319 50778 6223 19 127 NONE LEFT 8.7 40.9 81.3 -22.9 42.0 42.5 -0.05 233 334 44318 6325 22 127 NONE LEFT 8.7 43.0 75.7 -26.9 41.0 43.0 -0.20 225 335 39592 6424 72 78 NONE LEFT 8.8 44.5 69.5 -33.4 41.2 46.8 -0.56 269 305 39600 6443 127 0 NONE LEFT 8.9 46.3 65.3 -34.8 42.0 48.1 -0.64 337 300 39791 6450 127 0 NONE LEFT 9.0 45.7 65.1 -33.1 41.0 46.2 -0.52 313 295 39873 6450 127 0 NONE LEFT 9.0 43.6 65.0 -30.4 40.8 43.7 -0.29 270 299 39847 6462 127 21 NONE LEFT 9.1 42.1 65.2 -27.8 41.4 42.1 -0.07 222 305 39789 6492 21 127 NONE LEFT 9.1 41.0 65.0 -25.9 42.7 42.2 0.05 207 342 39744 6634 0 127 NONE LEFT 9.1 41.0 65.4 -24.8 44.3 42.2 0.05 206 342 39697 6784 0 127 NONE LEFT 9.2 43.8 65.4 -24.7 44.3 42.7 0.16 206 345 39649 6942 0 127 NONE LEFT 9.2 45.0 65.7 -25.0 46.5 44.7 0.18 203 318 39598 6997 0 127 NONE LEFT 9.2 46.2 65.0 -26.0 48.7 46.0 0.27 204 344 39545 7134 0 127 NONE LEFT 9.3 45.8 64.1 -27.7 49.1 46.3 0.28 201 318 39490 7190 0 127 NONE LEFT 9.4 46.1 62.5 -31.1 48.5 46.6 0.19 217 337 39415 7295 0 127 NONE LEFT 9.4 48.1 58.9 -35.5 48.4 47.1 0.13 283 313 39357 7319 0 127 NONE LEFT 9.5 46.5 55.9 -42.1 47.8 47.5 0.03 526 287 29418 7261 0 127 NONE LEFT 9.6 48.4 53.4 -50.2 47.1 47.6 -0.05 889 256 36832 7131 0 127 NONE LEFT 9.7 48.2 53.4 -57.6 46.8 48.4 -0.16 394 201 37149 7067 16 127 NONE LEFT 9.7 48.7 53.9 -60.9 46.8 49.5 -0.27 267 201 36992 6990 75 74 NONE LEFT 9.7 48.7 53.9 -64.0 46.8 49.5 -0.27 262 190 36858 6885 78 71 NONE LEFT 9.8 50.4 54.5 -66.8 47.7 50.5 -0.28 184 172 36023 6802 75 74 NONE LEFT 9.8 50.4 55.6 -68.9 48.1 50.5 -0.28 151 172 24201 6723 75 75 NONE LEFT 9.8 52.9 55.6 -70.0 48.1 53.4 -0.53 139 173 19940 6618 127 0 NONE LEFT 9.9 53.1 56.2 -70.7 49.1 54.0 -0.49 130 174 19955 6622 127 0 NONE LEFT 9.9 52.3 58.9 -70.6 49.6 54.9 -0.53 129 174 19970 6684 127 0 NONE LEFT 9.9 50.7 64.6 -69.5 47.8 52.7 -0.49 133 175 19967 6738 127 0 NONE LEFT 10.0 49.7 163.3 -67.8 47.1 50.4 -0.33 134 179 19949 6729 119 30 NONE LEFT 10.0 48.4 205.8 -65.3 46.1 49.3 -0.32 154 181 19869 6648 110 39 NONE LEFT 10.1 48.4 122.2 -62.7 44.3 47.7 -0.34 233 187 19733 6558 123 26 NONE LEFT 10.1 45.1 104.6 -60.3 44.3 46.6 -0.23 247 190 19596 6440 81 68 NONE LEFT 10.2 44.8 77.0 -57.9 42.6 45.4 -0.28 251 198 19447 6308 108 41 NONE LEFT 10.3 43.5 70.1 -55.9 41.9 44.3 -0.24 337 207 19530 6146 96 53 NONE LEFT 10.3 42.5 71.3 -54.5 40.0 43.0 -0.22 357 215 19664 5953 89 60 NONE LEFT 10.4 41.3 78.3 -53.1 39.4 41.9 -0.25 368 221 19805 5750 112 37 NONE LEFT 10.5 40.8 136.4 -51.0 39.3 41.5 -0.25 699 220 23796 5542 120 29 NONE LEFT 10.5 40.4 154.6 -48.7 38.3 40.5 -0.22 724 225 27948 5353 104 46 NONE LEFT 10.6 40.1 150.2 -45.7 39.0 40.1 -0.11 784 243 32974 5152 53 96 NONE LEFT 10.7 40.2 147.4 -44.2 38.8 39.7 -0.09 849 276 39021 4959 43 106 NONE LEFT 10.8 40.1 149.9 -45.1 39.1 40.2 -0.11 853 276 44705 4765 50 99 NONE LEFT 10.9 40.7 161.1 -46.2 39.2 40.1 -0.09 815 253 49142 4547 43 106 NONE LEFT 11.0 40.4 176.8 -48.1 38.8 40.6 -0.18 770 242 52320 4346 79 70 NONE LEFT 11.1 41.2 195.2 -49.7 39.4 40.2 -0.08 717 241 54283 4190 29 120 NONE LEFT 11.1 40.5 213.7 -50.5 38.9 40.4 -0.15 678 236 55413 4033 62 87 NONE LEFT 11.2 40.9 234.6 -51.6 38.5 40.3 -0.18 668 231 56155 3937 81 69 NONE LEFT 11.3 40.4 230.8 -52.0 38.1 40.7 -0.26 662 234 56574 3835 121 28 NONE LEFT 11.3 40.0 254.1 -51.1 37.8 40.1 -0.23 663 237 56735 3844 107 42 NONE LEFT 11.4 39.7 265.4 -49.1 37.2 39.9 -0.27 671 244 57642 3638 127 17 NONE LEFT 11.5 39.0 250.4 -45.5 37.3 39.3 -0.20 749 254 59954 3308 98 51 NONE LEFT 11.6 38.9 242.7 -41.7 38.0 38.9 -0.09 809 300 62860 3267 53 96 NONE LEFT 11.7 39.3 220.9 -40.2 38.7 39.1 -0.04 596 307 62986 3240 27 122 NONE LEFT 11.8 40.3 220.4 -41.3 39.3 39.2 0.01 608 246 63061 3105 2 127 NONE LEFT 11.9 40.7 247.4 -43.9 39.6 40.1 -0.05 711 274 63914 2973 21 127 NONE LEFT 11.9 40.9 265.1 -48.0 39.4 40.6 -0.12 757 266 65459 2914 52 97 NONE LEFT 12.0 41.1 270.7 -51.5 39.6 40.9 -0.13 653 235 66130 2864 50 99 NONE LEFT 12.1 41.5 287.8 -54.8 39.1 41.5 -0.24 483 240 65799 2816 106 43 NONE LEFT 12.2 41.8 295.3 -56.0 38.9 41.4 -0.25 289 246 65670 2758 113 36 NONE LEFT 12.2 42.3 299.4 -56.1 39.3 41.4 -0.21 276 256 65237 2688 87 62 NONE LEFT 12.2 41.6 274.6 -55.5 38.4 41.6 -0.32 281 259 64625 2496 127 1 NONE LEFT 12.3 40.9 297.8 -55.3 38.4 40.8 -0.24 280 247 64006 2291 119 31 NONE LEFT 12.4 39.9 299.4 -52.3 37.5 40.3 -0.28 456 239 62763 2065 127 6 NONE LEFT 12.4 39.9 295.6 -49.8 36.6 39.7 -0.17 612 228 61871 1935 88 61 NONE LEFT 12.5 38.4 290.4 -46.0 36.7 38.2 -0.15 667 271 61220 1723 78 71 NONE LEFT 12.6 38.7 277.8 -44.2 37.8 38.0 -0.02 734 296 60939 1659 29 121 NONE LEFT 12.7 38.5 281.4 -44.1 38.0 38.1 -0.01 768 265 60962 1467 22 127 NONE LEFT 12.8 38.9 273.8 -45.9 37.8 38.3 -0.05 726 188 61016 1376 35 114 NONE LEFT 12.8 39.1 285.2 -49.3 38.3 39.2 -0.09 665 330 61507 1506 49 100 NONE LEFT 12.9 39.6 289.8 -51.6 38.5 39.2 -0.07 606 298 58771 1580 37 112 NONE LEFT 13.0 38.9 86.6 -53.5 37.7 39.0 -0.13 575 271 58624 1587 69 80 NONE LEFT 13.1 39.6 73.4 -55.4 37.3 39.7 -0.24 342 228 58015 1566 114 35 NONE LEFT 13.1 39.5 69.5 -54.8 36.4 39.2 -0.28 301 261 57661 1541 127 6 NONE LEFT 13.2 38.2 69.2 -52.4 36.2 37.9 -0.17 427 231 55661 1460 95 54 NONE LEFT 13.3 37.7 69.3 -49.7 35.3 37.4 -0.21 562 267 53008 1393 118 31 NONE LEFT 13.4 37.6 69.4 -46.6 158.7 38.0 1.00 638 304 50245 1388 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 13.4: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 8.03 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 13.9: gl_Left/RightCenterCm = 263.1/69.9, Left/RightSteerVal = -1.00/0.01 In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase() 14056: gl_Left/RightCenterCm = 262.8/69.7, Left/RightSteerVal = -1.00/-0.02 14057: gl_Left/RightCenterCm = 262.8/69.7, Left/RightSteerVal = -1.00/-0.02 Calling RotateToParallelOrientation(TRACKING_RIGHT) In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 70.02/69.81/0.02 TrackRightWallOffset(350.0, 0.0, 20.0, 30) called TrackRightWallOffset: Start tracking offset of 30cm at 16.1 Calling CaptureWallOffset(TRACKING_RIGHT, 69.2) Hdg Dist SpinTurn: Init hdg = -40.00 deg, Turn = 90.00 deg, tgt = 50.00 deg, timeout = 4.00 sec After SpinTurn, Hdg/Dist = 48.5/71 MTFD: at start, tgt = 30cm, curr_dist = 71, front/rear var = 83325.0/13328.0 MTFD: Stopped with front dist = 29 SpinTurn: Init hdg = 53.99 deg, Turn = 90.00 deg, tgt = -36.01 deg, timeout = 4.00 sec In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 12.23/13.26/-0.10 Calling SpinTurn() for angle = -5.9 SpinTurn: Init hdg = -37.27 deg, Turn = 5.89 deg, tgt = -43.15 deg, timeout = 1.00 sec RTPO: ending front/rear/steer avgs = 11.89/12.30/-0.04 RTPO: GetWallOrientDeg(-0.04) returned ending orientation of -2.34 deg End of CaptureWallOffset Just after CaptureWallOffset(TRACKING_RIGHT, 14.3) Sec LCen RCen Deg RF RR RStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 26.1 175.5 13.8 -42.1 11.8 12.2 -0.04 494 168 127834 4532 0 127 NONE RIGHT 26.2 222.8 11.3 -44.6 12.1 12.3 -0.02 496 166 127800 4599 0 127 NONE RIGHT 26.3 153.6 12.0 -50.3 12.4 12.3 0.00 598 171 128247 4664 0 127 NONE RIGHT 26.3 101.9 11.9 -53.8 12.4 11.8 0.06 566 176 128369 4730 0 127 NONE RIGHT 26.4 96.7 12.2 -57.6 13.1 11.8 0.13 555 176 128308 4805 0 127 NONE RIGHT 26.4 94.3 14.1 -65.0 15.3 13.5 0.18 451 167 127718 4906 17 127 NONE RIGHT 26.5 94.4 15.6 -69.5 16.4 13.7 0.27 275 162 127236 5025 48 101 NONE RIGHT 26.5 95.5 15.6 -73.6 16.4 13.7 0.27 229 162 126748 5138 50 99 NONE RIGHT 26.5 99.1 16.1 -76.4 18.7 15.4 0.33 214 166 126031 5231 70 79 NONE RIGHT 26.6 102.6 17.7 -78.8 21.0 16.1 0.49 248 166 124746 5313 117 32 NONE RIGHT 26.6 102.6 17.7 -80.3 21.0 16.1 0.49 283 174 122933 5361 120 29 NONE RIGHT 26.6 107.3 19.5 -81.1 23.9 17.1 0.68 288 174 120707 5391 127 0 NONE RIGHT 26.7 109.4 21.3 -81.0 23.5 18.9 0.46 289 180 118048 5383 127 16 NONE RIGHT 26.7 109.4 22.8 -80.0 24.8 20.9 0.39 288 185 114924 5342 121 28 NONE RIGHT 26.8 109.2 22.8 -78.7 25.7 20.8 0.49 279 186 111311 5266 127 9 NONE RIGHT 26.8 103.9 24.1 -76.6 27.0 22.0 0.50 232 186 107270 5155 127 0 NONE RIGHT 26.8 100.9 25.0 -74.5 28.0 22.9 0.51 197 187 102726 5004 127 0 NONE RIGHT 26.9 96.4 25.6 -70.7 28.3 23.6 0.47 201 186 97456 4808 127 0 NONE RIGHT 27.0 93.7 26.4 -66.9 28.7 24.1 0.46 238 184 91339 4565 127 0 NONE RIGHT 27.0 91.7 26.4 -60.8 27.9 24.5 0.37 301 182 84454 4268 127 7 NONE RIGHT 27.1 91.0 26.4 -56.3 27.9 25.8 0.21 516 185 78078 3907 108 41 NONE RIGHT 27.1 112.7 26.4 -52.4 27.9 25.8 0.21 527 189 71199 3477 106 43 NONE RIGHT 27.2 262.2 26.5 -46.9 27.6 26.2 0.14 618 201 64846 2972 90 60 NONE RIGHT 27.3 211.2 29.2 -42.0 27.9 28.1 -0.02 583 196 57367 2393 62 87 NONE RIGHT 27.3 199.0 28.1 -40.4 27.9 28.1 -0.02 457 199 47772 1733 54 95 NONE RIGHT 27.4 197.0 27.3 -40.2 28.2 28.4 -0.02 460 215 37397 999 47 102 NONE RIGHT 27.4 192.8 27.2 -40.8 27.1 27.8 -0.07 451 212 37910 172 30 119 NONE RIGHT 27.5 193.9 26.8 -41.3 27.3 27.6 -0.03 450 214 38302 187 36 113 NONE RIGHT 27.6 191.1 26.4 -42.7 26.9 27.5 -0.06 438 212 38484 198 26 123 NONE RIGHT 27.6 197.1 26.1 -44.6 26.0 27.0 -0.10 453 208 38670 206 12 127 NONE RIGHT 27.7 200.9 25.2 -47.8 27.9 26.4 0.15 672 203 41360 209 81 68 NONE RIGHT 27.8 202.7 26.5 -50.5 25.4 25.8 -0.04 624 199 43041 209 36 114 NONE RIGHT 27.8 204.4 27.1 -51.9 28.0 26.4 0.16 526 200 43331 210 99 51 NONE RIGHT 27.8 207.3 25.2 -52.6 26.2 25.2 0.10 499 205 43192 213 76 73 NONE RIGHT 27.9 207.3 27.7 -53.1 28.2 26.3 0.19 493 205 42836 216 104 45 NONE RIGHT 27.9 200.1 27.7 -53.2 28.2 26.3 0.19 491 209 42293 221 106 43 NONE RIGHT 27.9 201.3 27.2 -53.1 26.5 26.8 -0.03 490 212 41558 229 47 102 NONE RIGHT 28.0 195.9 26.3 -52.8 27.5 25.5 0.20 492 212 40665 237 99 50 NONE RIGHT 28.0 195.9 26.3 -52.9 27.9 25.5 0.20 488 208 39571 243 103 47 NONE RIGHT 28.0 189.8 26.3 -53.0 27.9 25.9 0.20 487 211 38285 251 103 47 NONE RIGHT 28.1 151.1 27.2 -52.5 28.0 26.2 0.18 488 209 36809 256 109 40 NONE RIGHT 28.1 151.1 26.9 -52.2 27.7 27.0 0.07 488 209 35151 260 73 76 NONE RIGHT 28.2 100.5 27.4 -51.4 28.3 27.4 0.09 495 205 33350 263 84 65 NONE RIGHT 28.2 81.8 27.2 -51.0 28.3 27.4 0.09 510 203 31436 264 85 64 NONE RIGHT 28.3 79.8 27.3 -50.7 28.7 27.7 0.10 526 199 29407 264 88 61 NONE RIGHT 28.3 77.9 28.0 -50.7 28.8 27.9 0.14 548 198 27278 264 102 47 NONE RIGHT 28.4 77.1 28.3 -50.5 29.4 28.2 0.12 555 193 24949 262 96 53 NONE RIGHT 28.5 77.3 27.9 -49.8 29.1 27.9 0.12 564 196 22423 258 89 60 NONE RIGHT 28.5 76.9 28.5 -49.3 29.0 28.4 0.06 578 197 19709 254 82 67 NONE RIGHT 28.6 77.0 28.0 -49.0 29.1 29.4 -0.03 577 196 16718 249 52 97 NONE RIGHT 28.6 76.7 28.5 -48.9 29.7 29.0 0.07 576 190 17009 238 83 66 NONE RIGHT 28.7 77.0 28.5 -48.9 29.8 29.3 0.05 570 195 17262 225 78 71 NONE RIGHT 28.8 77.9 29.0 -48.7 29.4 28.9 0.05 576 194 17140 215 85 64 NONE RIGHT 28.8 78.8 29.2 -48.1 29.6 29.6 0.04 567 193 17145 209 82 67 NONE RIGHT 28.9 81.7 29.4 -47.6 29.4 29.3 0.01 584 193 17285 202 72 77 NONE RIGHT 28.9 84.6 28.9 -47.4 29.6 29.7 -0.01 598 198 17723 187 58 91 NONE RIGHT 29.0 88.9 29.1 -47.2 29.7 29.8 -0.01 590 196 17443 165 64 85 NONE RIGHT 29.0 92.6 29.4 -47.2 30.3 29.4 0.08 588 200 16683 143 94 55 NONE RIGHT 29.1 103.7 29.2 -47.2 30.0 29.4 0.06 579 197 15629 125 89 60 NONE RIGHT 29.2 107.8 29.5 -47.1 29.3 29.9 -0.06 573 195 14784 106 49 100 NONE RIGHT 29.2 111.7 28.9 -46.6 29.9 29.2 0.07 575 197 14168 94 83 66 NONE RIGHT 29.3 118.9 29.4 -46.3 29.6 29.4 0.08 578 191 13535 83 95 54 NONE RIGHT 29.3 130.5 28.7 -45.8 29.3 29.3 0.00 577 197 12839 77 63 87 NONE RIGHT 29.4 135.5 28.6 -45.3 29.6 29.2 0.04 581 201 12082 73 74 75 NONE RIGHT 29.5 143.9 29.0 -45.2 29.1 29.5 0.01 580 196 11172 70 71 78 NONE RIGHT 29.5 155.8 29.2 -44.8 29.3 29.9 -0.06 581 206 9705 67 48 101 NONE RIGHT 29.6 166.1 28.6 -44.7 28.8 29.3 0.03 579 207 7704 66 70 79 NONE RIGHT 29.7 222.9 28.1 -45.0 29.0 29.5 -0.05 572 212 5628 65 45 104 NONE RIGHT 29.7 237.1 29.0 -45.4 29.0 29.3 -0.03 558 192 3896 61 56 93 NONE RIGHT 29.8 252.5 28.6 -46.2 28.6 28.9 0.00 555 192 2767 56 60 89 NONE RIGHT 29.8 253.9 28.1 -46.6 29.1 28.6 0.05 544 193 2754 52 77 72 NONE RIGHT 29.9 253.3 28.9 -46.8 28.9 28.6 0.03 543 186 2749 53 71 78 NONE RIGHT 30.0 260.3 28.2 -47.1 28.8 29.2 -0.04 540 193 2633 55 48 101 NONE RIGHT 30.0 259.3 28.4 -47.3 28.9 28.3 0.06 527 184 2600 59 80 70 NONE RIGHT 30.1 259.8 28.1 -47.5 28.7 28.8 -0.01 532 184 2462 65 58 91 NONE RIGHT 30.1 262.3 28.5 -47.6 29.3 28.6 0.07 520 183 2337 65 83 66 NONE RIGHT 30.2 262.5 28.1 -47.8 30.7 28.3 0.24 518 179 2180 71 127 21 NONE RIGHT 30.2 261.8 28.7 -48.0 28.9 28.4 0.05 509 179 2027 73 81 68 NONE RIGHT 30.3 259.9 28.9 -47.0 28.8 29.1 -0.03 506 179 1826 77 52 97 NONE RIGHT 30.3 213.0 28.4 -46.5 28.8 28.6 0.08 523 181 1661 80 86 63 NONE RIGHT 30.4 86.6 28.5 -46.2 29.2 28.7 0.01 522 186 1356 81 65 84 NONE RIGHT 30.4 80.5 28.2 -45.8 31.6 28.6 0.30 515 179 1244 87 127 16 NONE RIGHT 30.5 78.4 28.5 -45.3 28.9 28.3 0.06 518 182 1250 91 85 64 NONE RIGHT 30.5 75.7 28.5 -44.2 28.7 28.6 0.01 523 182 1220 93 65 84 NONE RIGHT 30.6 75.8 27.8 -43.1 28.6 28.7 -0.01 459 189 1311 92 51 98 NONE RIGHT 30.7 76.9 27.9 -43.2 30.2 28.5 0.17 367 201 1866 89 103 46 NONE RIGHT 30.7 77.4 27.6 -43.8 28.3 28.6 -0.03 503 200 1844 85 47 102 NONE RIGHT 30.8 78.2 29.3 -43.0 28.2 28.1 0.01 446 188 1974 79 69 80 NONE RIGHT 30.9 78.6 29.0 -42.9 26.9 27.6 -0.07 352 200 2614 76 45 104 NONE RIGHT 31.0 78.4 27.0 -42.9 27.5 27.7 -0.02 354 220 3224 84 46 103 NONE RIGHT 31.0 78.6 26.0 -43.5 27.0 27.1 -0.01 415 214 3462 87 43 106 NONE RIGHT 31.1 78.6 28.2 -44.3 29.2 27.2 0.20 488 204 3462 85 112 37 NONE RIGHT 31.1 79.2 26.2 -45.0 26.5 26.8 -0.03 487 197 3475 83 41 108 NONE RIGHT 31.2 78.9 26.2 -44.7 26.2 26.9 -0.07 487 188 3506 82 23 126 NONE RIGHT 31.3 79.8 26.1 -45.5 26.9 26.2 0.07 482 193 3553 81 68 81 NONE RIGHT 31.3 79.2 26.0 -46.4 31.8 26.6 0.52 472 184 3614 83 127 0 NONE RIGHT RightSteer = 1.0, result = TRUE 31.4 79.5 29.8 -47.3 44.3 25.9 1.00 465 185 3680 84 127 0 EXCESS_STEER_VAL RIGHT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 31.4: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 8.02 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 31.8: gl_Left/RightCenterCm = 79.6/48.4, Left/RightSteerVal = -0.17/0.37 In HandleAnomalousConditions(RIGHT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(RIGHT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase() 32503: gl_Left/RightCenterCm = 81.0/117.5, Left/RightSteerVal = -0.12/0.26 32503: gl_Left/RightCenterCm = 81.0/117.5, Left/RightSteerVal = -0.12/0.26 Calling ChooseBetterTrackingSide() In ChooseBetterTrackingSide with avg hdg = -46.9 TurnToHdgDeg: currHdg = -44.8, tgtHdg = -46.9, retries = 0 SpinTurn: Init hdg = -44.66 deg, Turn = 2.12 deg, tgt = -46.78 deg, timeout = 1.00 sec SpinTurn: Init hdg = -46.61 deg, Turn = 10.00 deg, tgt = -56.61 deg, timeout = 1.00 sec SpinTurn: Init hdg = -57.43 deg, Turn = 10.00 deg, tgt = -67.43 deg, timeout = 1.00 sec SpinTurn: Init hdg = -68.25 deg, Turn = 10.00 deg, tgt = -78.25 deg, timeout = 1.00 sec SpinTurn: Init hdg = -79.75 deg, Turn = 10.00 deg, tgt = -89.75 deg, timeout = 1.00 sec SpinTurn: Init hdg = -89.68 deg, Turn = 10.00 deg, tgt = -99.68 deg, timeout = 1.00 sec SpinTurn: Init hdg = -99.18 deg, Turn = 10.00 deg, tgt = -109.18 deg, timeout = 1.00 sec SpinTurn: Init hdg = -109.15 deg, Turn = 10.00 deg, tgt = -119.15 deg, timeout = 1.00 sec SpinTurn: Init hdg = -118.77 deg, Turn = 10.00 deg, tgt = -128.77 deg, timeout = 1.00 sec SpinTurn: Init hdg = -128.08 deg, Turn = 10.00 deg, tgt = -138.08 deg, timeout = 1.00 sec TurnToHdgDeg: currHdg = -138.1, tgtHdg = -46.9, retries = 0 SpinTurn: Init hdg = -138.38 deg, Turn = 91.23 deg, tgt = -47.15 deg, timeout = 4.05 sec ChooseBetterTrackingSide 'left' block with numleft/rightwallsegments = 4/1 ChooseBetterTrackingSide() returned LEFT In RotateToParallelOrientation(LEFT) SpinTurn: Init hdg = -45.87 deg, Turn = 7.49 deg, tgt = -38.38 deg, timeout = 1.00 sec TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 42.0 Calling CaptureWallOffset(TRACKING_LEFT, 83.2) Hdg Dist SpinTurn: Init hdg = -39.50 deg, Turn = 90.00 deg, tgt = -129.50 deg, timeout = 4.00 sec After SpinTurn, Hdg/Dist = -127.9/89 MTFD: at start, tgt = 30cm, curr_dist = 90, front/rear var = 83325.0/13328.0 MTFD: Stopped with front dist = 29 SpinTurn: Init hdg = -129.14 deg, Turn = 90.00 deg, tgt = -39.14 deg, timeout = 4.00 sec In RotateToParallelOrientation(LEFT) End of CaptureWallOffset Just after CaptureWallOffset(TRACKING_LEFT, 11.8) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 49.7 12.0 166.2 -38.0 12.6 12.3 0.03 374 57 132811 10857 127 0 NONE LEFT 49.7 12.0 167.0 -37.2 12.2 12.4 -0.02 387 56 131315 11196 127 0 NONE LEFT 49.8 11.9 166.2 -34.7 12.3 12.5 -0.02 424 55 129634 11494 127 0 NONE LEFT 49.9 12.2 146.7 -30.6 12.5 12.5 0.00 469 49 127856 11779 127 0 NONE LEFT 49.9 12.9 136.2 -25.2 13.6 12.2 0.14 495 36 125927 12075 127 0 NONE LEFT 50.0 13.2 130.0 -21.2 14.6 13.0 0.16 240 36 123272 12299 127 4 NONE LEFT 50.0 13.2 123.8 -17.0 14.6 13.0 0.16 241 30 120122 12472 127 4 NONE LEFT 50.0 13.8 123.8 -12.6 16.6 13.5 0.31 210 26 116540 12578 102 47 NONE LEFT 50.1 17.2 121.6 -6.1 19.8 13.7 0.49 249 23 112259 12603 33 116 NONE LEFT 50.1 17.2 123.2 -3.1 19.8 15.1 0.47 238 20 107390 12538 36 113 NONE LEFT 50.2 21.9 126.0 -1.8 26.3 19.1 0.72 270 19 101839 12367 0 127 NONE LEFT 50.3 23.9 126.2 -2.9 28.9 21.8 0.71 273 21 95619 12061 0 127 NONE LEFT 50.3 24.7 122.9 -4.0 29.7 22.3 0.74 254 23 88676 11614 0 127 NONE LEFT 50.3 26.6 119.9 -5.9 31.4 26.0 0.54 227 26 80958 11014 0 127 NONE LEFT 50.4 28.1 118.9 -8.4 32.1 26.8 0.53 225 29 72393 10256 0 127 NONE LEFT 50.4 29.0 119.0 -12.1 32.3 27.4 0.49 221 33 62930 9330 0 127 NONE LEFT 50.5 28.9 119.5 -16.3 31.8 27.4 0.44 207 37 52508 8229 0 127 NONE LEFT 50.5 29.5 122.2 -20.9 31.1 28.5 0.26 197 43 41062 6947 10 127 NONE LEFT 50.6 29.1 126.4 -25.9 30.5 28.3 0.22 218 49 28572 5480 18 127 NONE LEFT 50.7 29.1 149.2 -32.8 29.9 28.6 0.20 356 58 15722 3820 25 124 NONE LEFT 50.7 29.3 144.8 -37.6 29.6 28.5 0.14 427 69 17091 1968 39 110 NONE LEFT 50.8 29.3 144.2 -42.1 29.1 29.0 0.01 376 85 17886 1968 76 73 NONE LEFT 50.8 29.3 148.5 -45.4 28.2 29.0 0.01 294 112 18077 1983 78 71 NONE LEFT 50.8 29.1 98.0 -47.4 28.4 29.4 -0.12 223 169 17994 2123 127 22 NONE LEFT 50.9 29.8 27.8 -48.3 28.0 29.9 -0.15 230 219 17906 2465 127 9 NONE LEFT 50.9 29.8 13.6 -47.7 28.6 29.9 -0.19 237 227 17810 2841 127 0 NONE LEFT 51.0 30.0 9.8 -46.3 28.3 30.2 -0.16 219 225 17658 3191 127 10 NONE LEFT 51.0 29.9 266.6 -44.1 28.4 30.1 -0.17 224 232 17491 3563 127 2 NONE LEFT 51.1 29.8 22.0 -41.5 29.1 30.9 -0.18 292 220 17491 3855 127 0 NONE LEFT 51.1 29.8 221.6 -38.4 28.7 30.0 -0.13 355 174 17774 3964 127 14 NONE LEFT 51.2 30.4 125.4 -34.3 30.2 30.3 -0.01 386 132 18198 3983 81 68 NONE LEFT 51.2 31.7 157.5 -30.6 30.7 31.0 -0.03 436 108 18948 3983 78 71 NONE LEFT 51.3 32.3 181.0 -27.9 32.1 31.5 0.06 438 90 19598 3985 42 107 NONE LEFT 51.3 33.0 172.4 -27.0 33.4 33.0 0.04 211 80 19172 3992 40 110 NONE LEFT 51.4 33.9 167.4 -27.2 34.9 32.8 0.21 207 75 18714 4002 0 127 NONE LEFT 51.4 34.7 149.9 -28.2 35.9 34.3 0.10 197 75 18223 4007 17 127 NONE LEFT 51.5 36.3 166.1 -29.7 36.8 34.4 0.15 431 79 18499 4006 0 127 NONE LEFT 51.5 36.3 146.6 -30.9 36.8 35.3 0.15 430 82 18643 3998 0 127 NONE LEFT 51.6 37.0 171.2 -34.2 36.7 36.7 0.00 412 87 18535 3982 23 126 NONE LEFT 51.6 37.2 200.9 -37.6 36.9 36.5 0.04 366 95 18052 3960 12 127 NONE LEFT 51.7 37.3 215.0 -40.5 36.6 36.7 -0.01 347 108 17391 3936 28 121 NONE LEFT 51.7 37.3 71.0 -44.2 35.9 37.6 -0.17 271 132 16487 3932 95 54 NONE LEFT 51.8 37.3 17.8 -47.6 35.9 37.6 -0.17 223 167 15557 4000 99 50 NONE LEFT 51.8 38.2 238.1 -48.9 36.1 38.2 -0.21 174 258 14720 4475 112 37 NONE LEFT 51.9 38.2 328.2 -49.0 36.3 38.2 -0.21 170 255 13871 4916 113 36 NONE LEFT 51.9 38.2 41.4 -48.2 36.8 38.1 -0.13 171 258 12990 5360 73 76 NONE LEFT 52.0 37.5 11.6 -47.4 36.5 37.5 -0.10 171 258 12072 5793 68 81 NONE LEFT 52.0 37.6 344.3 -47.1 35.7 37.8 -0.21 176 265 11094 6259 117 32 NONE LEFT 52.1 38.0 282.5 -46.9 35.7 37.6 -0.19 173 253 10088 6639 100 49 NONE LEFT 52.1 38.1 43.3 -46.2 35.8 37.5 -0.17 186 259 8991 7038 92 57 NONE LEFT 52.2 37.5 208.1 -45.0 36.4 37.7 -0.13 242 273 8860 7477 79 70 NONE LEFT 52.2 37.2 278.6 -44.3 35.9 37.3 -0.14 251 258 8651 7797 81 68 NONE LEFT 52.3 37.7 266.8 -43.6 36.6 37.9 -0.13 240 234 8249 7959 78 71 NONE LEFT 52.3 37.7 313.2 -43.1 36.7 37.5 -0.08 232 224 7516 8034 55 94 NONE LEFT 52.4 37.8 207.9 -43.3 37.8 36.9 -0.04 231 209 6514 7991 40 109 NONE LEFT 52.4 37.8 208.0 -43.7 37.8 38.5 -0.07 234 212 6521 7929 49 100 NONE LEFT 52.5 38.3 292.9 -44.2 37.1 38.3 -0.12 234 215 6529 7831 67 83 NONE LEFT 52.5 38.5 177.3 -44.8 36.9 38.9 -0.18 238 208 6482 7671 94 55 NONE LEFT 52.6 38.5 301.6 -45.0 37.0 38.7 -0.17 237 231 6493 7539 92 57 NONE LEFT 52.6 38.4 329.6 -44.7 37.6 38.1 -0.05 230 221 6503 7326 39 110 NONE LEFT 52.7 38.4 298.4 -44.1 37.8 37.5 0.03 220 218 6543 7059 10 127 NONE LEFT 52.7 39.2 330.4 -44.0 41.0 38.6 0.24 214 215 6591 6767 0 127 NONE LEFT 52.8 41.2 24.2 -45.0 56.7 39.5 1.00 219 215 6628 6456 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 52.8: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 8.01 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 53.2: gl_Left/RightCenterCm = 57.1/247.9, Left/RightSteerVal = 1.00/-1.00 In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase() 53857: gl_Left/RightCenterCm = 105.2/83.3, Left/RightSteerVal = -1.00/-1.00 53857: gl_Left/RightCenterCm = 105.2/83.3, Left/RightSteerVal = -1.00/-1.00 53864: HandleExcessSteervalCase Open Corner Block: gl_Left/RightCenterCm = 105.2/83.3, Left/RightSteerVal = -1.00/-1.00 HandleExcessSteervalCase in 'else //OpenCorner?': Can't decide what to to - quitting |
Stay Tuned!
Frank
13 September 2023 Update:
Yesterday, at the end of the run the robot quit with the output “HandleExcessSteervalCase in ‘else //OpenCorner:’ Can’t decide what to do.” However, when I tried to duplicate this problem in my office with a simple ‘L’ (open corner) configuration, the robot instead chose the ‘ChooseBetterTrackingSide() option – rats.
Looking through the code and the results, I can see this happened because the steervals from both sides were < MAX. The distances from both sides were > MAX_TRACKING_DIST_CM, but I am currently not using this factor for decisions.
Looks like I need to use both distance and steerval to distinguish among the various error configurations (or maybe I need to use the previous steerval, the one that caused the anomaly?
So, just using current steerval, we get the following four possibilities
- 1: Left < MAX, Right < MAX
- 2: Left < MAX, Right > MAX
- 3: Left > MAX, Right < MAX
- 4: Left > MAX, Right > MAX
If we then add the previous steerval (the one that created the anomaly) into the mix we get eight – the above four with PrevLeft > MAX, and four with PrevRight > Max.
So, assuming that the anomaly was EXCESS_STEERVAL on the left side, then the first possibility above was true in the test case, so what should have happened is recognition of an ‘open corner’ case, with the previously tracked wall on the left side. This should produce a left 90º turn, a forward run of 0.5-1.0sec and then either track left or ‘choose best side’.
15 September 2023 Update:
After thinking about this problem for a while, I have come to the conclusion that I’m overthinking the ‘Excess Steerval’ and ‘Open Corner’ conditions. I now think that the ‘Open Corner’ condition can be uniquely defined as the condition where both left and right distances are > MAX_TRACKING_DIST_CM after an ‘Excess Steerval’ anomaly occurs. This means my earlier re-write of HandleExcessSteervalCase() to incorporate steerval information is incorrect – bummer!
Looking at the two wall diagrams above, it looks like there are only a few conditions that are possible after an ‘Excess Steerval’ anomaly:
- An ‘open doorway’ configuration can be identified uniquely because it will have a tracked-side distance > MAX_TRACKING_DIST_CM and a non-tracked side < MAX_TRACKING_DIST_CM.
- An ‘open corner’ configuration can be identified uniquely because it will have tracked-side and non-tracked side distances > MAX_TRACKING_DIST_CM.
- The ‘Which Wall?’ (ChooseBestWall) configuration can be identified uniquely because it will have a tracked-side distance < MAX_TRACKING_DIST_CM.
So I think this all boils down to the following flow chart:
Based on the above, it appears that my original version of HandleExcessSteervalCase() was closer to the mark than my 09/04/23 re-write – oops! Fortunately I kept the old version as a comment block, so I may be able to fix my ‘oops’ without too much agony.
After modifying (once again) my HandleExcessSteervalCase() function, I got the ‘open corner’ case working – at least I think I did. The following short video and telemetry printout shows the action.
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 |
0.0: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.94 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 0.5: gl_Left/RightCenterCm = 30.5/95.7, Left/RightSteerVal = -0.15/0.11 In HandleAnomalousConditions(NEITHER) with last anomaly code = NONE In HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE- 0.5: gl_Left/RightCenterCm = 30.5/96.4, Left/RightSteerVal = -0.17/0.17 gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset() TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 0.5 Just after CaptureWallOffset(TRACKING_LEFT, 30.5) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.6 31.1 95.9 0.1 29.2 30.9 -0.17 355 92 69011 11842 127 18 NONE LEFT 0.7 30.1 96.2 0.4 28.8 30.5 -0.17 351 91 65615 11460 127 8 NONE LEFT 0.7 30.5 95.0 2.0 29.1 30.1 -0.10 362 92 62438 11121 111 38 NONE LEFT 0.8 30.4 94.9 4.7 28.4 30.3 -0.19 371 98 59486 10812 127 0 NONE LEFT 0.8 30.3 94.5 7.2 28.7 29.6 -0.09 376 110 56780 10499 109 40 NONE LEFT 0.9 30.1 94.3 11.0 28.7 29.1 -0.04 397 122 54215 10189 90 60 NONE LEFT 0.9 29.8 95.8 13.9 28.8 30.0 -0.12 402 129 51879 9903 127 21 NONE LEFT 1.0 30.0 97.1 17.4 29.3 30.0 -0.07 415 130 49721 9652 101 48 NONE LEFT 1.0 30.3 99.2 19.9 29.8 29.7 0.01 436 132 47696 9438 73 76 NONE LEFT 1.1 32.3 98.8 23.7 31.3 30.5 0.08 604 136 45615 9248 35 114 NONE LEFT 1.2 33.7 99.9 24.0 34.1 31.9 0.22 577 141 43665 9083 0 127 NONE LEFT 1.3 34.2 98.9 22.5 33.6 32.7 0.09 590 143 41884 8943 14 127 NONE LEFT 1.3 34.7 96.2 18.7 34.1 33.6 0.05 518 147 40344 8825 28 121 NONE LEFT 1.4 35.6 94.0 16.4 34.0 33.7 0.03 423 149 39422 8735 29 120 NONE LEFT 1.4 35.0 92.0 13.9 34.2 34.1 0.01 401 151 38850 8669 36 113 NONE LEFT 1.5 35.7 92.9 11.3 34.3 34.5 -0.02 381 151 38624 8628 46 103 NONE LEFT 1.5 35.7 98.5 8.7 33.9 34.5 -0.02 373 153 38641 8607 47 103 NONE LEFT 1.5 35.4 98.5 6.5 33.9 34.9 -0.10 361 152 38922 8607 80 70 NONE LEFT 1.6 35.5 118.5 4.9 34.0 35.0 -0.10 350 148 39445 8639 82 68 NONE LEFT 1.6 36.3 127.4 3.8 33.7 35.2 -0.15 349 142 40100 8704 91 58 NONE LEFT 1.7 35.9 134.7 3.3 33.8 35.3 -0.15 340 136 40941 8800 99 50 NONE LEFT 1.7 36.0 140.9 3.4 33.4 35.6 -0.22 341 133 41851 8912 127 20 NONE LEFT 1.7 35.6 144.7 3.9 33.1 35.6 -0.25 335 138 42875 9014 127 2 NONE LEFT 1.8 35.6 147.1 4.5 33.5 35.6 -0.25 331 138 43968 9119 127 1 NONE LEFT 1.8 35.6 148.3 5.8 33.5 34.9 -0.14 329 147 45086 9200 99 51 NONE LEFT 1.9 35.7 148.3 7.7 33.2 34.9 -0.17 345 157 46039 9252 112 37 NONE LEFT 1.9 35.3 148.6 9.8 32.8 34.5 -0.17 348 165 46925 9283 113 36 NONE LEFT 2.0 34.9 145.2 12.1 33.0 34.1 -0.11 352 172 47712 9293 93 56 NONE LEFT 2.0 35.0 143.9 14.4 33.3 33.9 -0.06 360 177 48343 9285 62 87 NONE LEFT 2.1 35.1 141.1 15.8 33.6 33.6 -0.03 367 179 48806 9259 51 98 NONE LEFT 2.1 34.8 139.2 16.3 34.5 33.9 -0.03 371 182 49098 9211 57 92 NONE LEFT 2.2 35.6 137.8 16.0 34.1 34.5 -0.04 370 184 49224 9137 54 95 NONE LEFT 2.2 35.8 141.5 15.5 34.3 34.8 -0.05 363 188 49190 9033 57 92 NONE LEFT 2.3 36.1 147.3 15.1 34.3 35.1 -0.08 361 192 48929 8896 60 89 NONE LEFT 2.3 36.0 151.0 14.3 34.0 34.7 -0.07 352 194 48449 8725 57 92 NONE LEFT 2.4 36.3 153.2 13.7 34.6 35.4 -0.08 345 197 47700 8515 60 89 NONE LEFT 2.4 36.8 156.3 13.4 35.0 35.9 -0.09 347 199 46585 8263 64 85 NONE LEFT 2.5 36.5 159.9 13.2 35.3 35.9 -0.06 339 201 45127 7964 54 95 NONE LEFT 2.5 36.5 162.6 13.2 34.7 36.2 -0.15 338 202 43243 7615 90 59 NONE LEFT 2.6 36.7 162.7 13.1 35.6 36.0 -0.04 332 206 40917 7212 49 100 NONE LEFT 2.6 36.9 164.1 13.0 35.2 36.6 -0.14 332 207 38076 6751 86 63 NONE LEFT 2.7 36.9 161.6 12.5 36.0 36.3 -0.03 329 210 34690 6227 46 103 NONE LEFT 2.7 37.0 159.6 12.5 35.7 36.4 -0.07 326 212 30715 5637 50 99 NONE LEFT 2.8 37.2 154.2 12.2 36.0 36.9 -0.09 320 214 26115 4976 57 92 NONE LEFT 2.8 37.1 151.8 11.9 36.5 36.5 -0.04 308 218 20860 4241 41 109 NONE LEFT 2.9 37.3 142.4 11.4 36.5 36.7 -0.02 316 220 14823 3428 33 116 NONE LEFT 2.9 38.0 140.3 10.8 36.5 36.5 0.00 302 223 8031 2532 19 127 NONE LEFT 3.0 38.4 145.4 9.8 36.6 37.7 -0.11 304 224 4359 1546 61 88 NONE LEFT 3.0 38.9 158.9 8.4 36.6 38.0 -0.14 293 228 4479 1535 74 75 NONE LEFT 3.1 39.0 175.3 7.4 37.0 38.4 -0.14 290 226 4598 1507 68 81 NONE LEFT 3.1 38.5 178.8 6.9 36.7 38.2 -0.15 291 226 4716 1464 77 72 NONE LEFT 3.2 39.0 181.5 6.7 35.9 38.1 -0.22 285 227 4847 1404 108 41 NONE LEFT 3.2 38.7 183.0 6.9 36.3 38.0 -0.17 286 228 4975 1340 93 56 NONE LEFT 3.3 38.7 182.8 7.7 36.2 38.1 -0.19 286 231 5098 1281 99 50 NONE LEFT 3.3 38.1 184.1 8.6 35.8 37.6 -0.18 277 236 5242 1260 96 53 NONE LEFT 3.4 37.9 183.4 8.9 35.7 37.2 -0.15 275 239 5361 1265 86 63 NONE LEFT 3.4 37.6 184.2 9.8 35.5 37.3 -0.18 277 244 5454 1282 102 48 NONE LEFT 3.5 38.0 183.6 10.4 35.9 37.3 -0.14 272 247 5522 1302 76 73 NONE LEFT 3.5 37.8 182.9 11.0 35.5 37.0 -0.15 272 250 5511 1318 84 65 NONE LEFT 3.6 37.6 182.6 11.3 35.6 37.0 -0.14 270 252 4313 1340 82 67 NONE LEFT 3.6 37.3 181.9 11.7 35.4 37.3 -0.19 271 256 3297 1372 105 44 NONE LEFT 3.7 37.2 181.8 11.9 36.0 37.0 -0.12 268 257 2081 1401 76 73 NONE LEFT 3.7 37.7 182.6 12.2 35.4 37.0 -0.16 264 259 1451 1434 88 61 NONE LEFT 3.8 37.7 181.3 12.2 35.6 36.9 -0.13 263 265 1334 1474 79 70 NONE LEFT 3.8 37.9 182.2 12.8 36.1 37.1 -0.10 260 267 1284 1512 68 81 NONE LEFT 3.9 38.1 182.6 13.2 36.5 37.5 -0.10 259 268 1280 1542 61 88 NONE LEFT 3.9 38.4 182.1 13.3 36.9 37.3 -0.04 261 272 1279 1572 34 115 NONE LEFT 4.0 38.0 182.7 13.2 36.4 37.5 -0.11 257 273 1303 1597 62 87 NONE LEFT 4.0 38.6 182.8 13.2 37.0 37.7 -0.07 253 273 1346 1597 44 105 NONE LEFT 4.0 38.9 182.2 13.0 37.4 37.6 -0.02 247 278 1397 1580 27 123 NONE LEFT 4.1 38.9 183.6 12.5 37.2 38.4 -0.12 247 282 1454 1539 65 84 NONE LEFT 4.1 39.1 182.6 11.6 38.0 38.0 0.00 241 285 1517 1482 15 127 NONE LEFT 4.2 39.2 180.6 10.9 37.7 38.1 -0.04 234 286 1598 1427 25 124 NONE LEFT 4.2 39.4 181.8 9.2 37.4 38.3 -0.09 230 287 1688 1356 49 100 NONE LEFT 4.3 39.3 179.3 7.3 37.3 38.6 -0.13 224 288 1789 1298 63 86 NONE LEFT 4.3 39.5 187.3 5.7 37.0 38.5 -0.15 228 290 1845 1263 71 78 NONE LEFT 4.4 39.6 190.9 4.6 36.9 39.1 -0.17 224 285 1895 1227 84 65 NONE LEFT 4.4 39.7 191.5 4.0 37.1 39.2 -0.23 217 289 1943 1206 111 38 NONE LEFT 4.5 39.6 191.1 4.1 36.8 38.9 -0.21 212 286 1970 1177 106 43 NONE LEFT 4.5 39.5 187.5 4.8 36.7 38.4 -0.17 216 290 1947 1152 86 63 NONE LEFT 4.6 39.4 188.3 5.5 36.5 38.0 -0.15 217 295 1889 1136 72 77 NONE LEFT 4.6 38.8 188.2 6.1 36.4 37.5 -0.11 207 302 1842 1129 64 85 NONE LEFT 4.7 38.3 186.5 6.1 35.5 37.2 -0.17 209 302 1794 1123 90 59 NONE LEFT 4.7 38.3 185.1 6.1 35.7 36.9 -0.12 206 302 1743 1111 69 80 NONE LEFT 4.8 38.3 181.9 6.0 34.9 36.9 -0.20 204 303 1708 1100 101 49 NONE LEFT 4.8 37.4 176.1 6.2 35.6 37.4 -0.18 202 310 1683 1097 103 47 NONE LEFT 4.9 37.4 169.9 6.7 36.1 36.9 -0.08 197 314 1649 1101 56 93 NONE LEFT 4.9 37.2 161.6 7.4 39.1 36.3 0.28 191 314 1638 1099 0 127 NONE LEFT 5.0 37.6 158.1 7.2 50.0 36.6 1.00 190 318 1617 1100 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 5.0: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.92 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 5.4: gl_Left/RightCenterCm = 69.0/131.3, Left/RightSteerVal = 1.00/-0.32 In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase(LEFT) 6293: gl_Left/RightCenterCm = 148.0/114.0, Left/RightSteerVal = -0.37/-0.26 HandleExcessSteervalCase(LEFT) open corner block HandleExcessSteervalCase) open corner block: gl_Left/RightCenterCm = 38.4/96.9, Left/RightSteerVal = -0.16/-0.19 TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 9.4 Just after CaptureWallOffset(TRACKING_LEFT, 38.4) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 9.5 38.5 95.5 -85.7 36.2 38.1 -0.18 101 180 2259 1292 94 55 NONE LEFT 9.5 38.7 94.0 -85.7 36.5 37.9 -0.17 97 182 2550 1370 92 57 NONE LEFT 9.6 38.2 93.3 -85.5 36.4 37.5 -0.10 94 188 2857 1436 62 87 NONE LEFT 9.6 37.8 93.1 -85.1 36.6 37.6 -0.12 94 190 3101 1497 74 75 NONE LEFT 9.7 38.1 92.8 -85.0 36.3 37.2 -0.06 93 191 3357 1563 42 108 NONE LEFT 9.7 37.6 90.2 -84.7 35.4 37.7 -0.14 91 196 3582 1616 79 70 NONE LEFT 9.8 37.2 88.4 -84.8 35.6 37.0 -0.14 87 199 3827 1666 82 68 NONE LEFT 9.8 37.3 85.2 -84.8 35.5 36.4 -0.09 85 204 4058 1708 58 91 NONE LEFT 9.9 36.5 82.2 -84.7 35.3 35.9 -0.06 83 213 4262 1728 54 95 NONE LEFT 9.9 36.8 78.8 -84.8 35.2 35.7 -0.05 79 213 4471 1748 50 99 NONE LEFT 10.0 36.9 72.6 -84.7 35.3 35.5 -0.02 79 221 4642 1756 40 109 NONE LEFT 10.0 36.7 70.3 -85.0 35.1 36.0 -0.09 75 218 4799 1770 63 86 NONE LEFT 10.1 36.2 67.1 -85.6 34.9 35.9 -0.07 71 224 4971 1777 57 92 NONE LEFT 10.1 36.7 64.3 -86.5 35.0 35.6 -0.07 62 229 5160 1785 57 92 NONE LEFT 10.2 36.7 58.8 -87.0 34.8 36.3 -0.13 59 225 5320 1799 83 66 NONE LEFT 10.2 36.7 57.2 -86.8 34.3 35.4 -0.06 55 224 5481 1818 55 94 NONE LEFT 10.3 36.0 56.1 -86.7 33.9 35.4 -0.11 54 228 5609 1834 77 72 NONE LEFT 10.3 35.4 56.2 -86.8 33.9 36.2 -0.23 49 236 5731 1841 127 12 NONE LEFT 10.4 35.4 56.8 -86.7 33.6 34.8 -0.09 46 242 5824 1845 75 74 NONE LEFT 10.4 35.3 59.0 -86.1 33.6 34.4 -0.08 45 240 5894 1850 68 81 NONE LEFT 10.5 34.9 61.0 -85.6 33.2 34.4 -0.12 45 250 5942 1850 94 55 NONE LEFT 10.5 35.7 63.2 -85.4 33.0 34.4 -0.14 43 257 5964 1850 96 54 NONE LEFT 10.6 35.0 65.3 -85.0 32.8 34.7 -0.19 41 254 5970 1848 119 30 NONE LEFT 10.6 35.0 64.8 -84.4 32.8 34.4 -0.16 41 263 5941 1847 104 46 NONE LEFT |
Here is the current ‘state of code’ for the ‘HandleExcessSteervalCase’ function:
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 |
bool HandleExcessSteervalCase(WallTrackingCases trkcase)//09/16/23 added trkcase to sig { //Purpose: Respond to an 'Excess Steerval' anomaly //Inputs: trkcase = WallTrackingCases enum denoting side being tracked //Outputs: Robot changes walls or other response //Plan: //Step1: Update all environmental parameters and wait for everything to settle //Step2: Handle Left side case //Step3: Handle Right side case //Notes: // 09/04/23 need to add steering value into decision tree. Now have 4 cases vs 2 // 09/16/23 rewritten per flowchart in 15 Sep 23 update to https://www.fpaynter.com/2023/09/responding-to-steerval-out-of-range-anomalies/ // 09/16/23 added WallTrackingCases trkcase to sig bool result = true; //Step1: Update all distances and wait for everything to settle gl_pSerPort->printf("Top of HandleExcessSteervalCase(%s)\n", WallTrackStrArray[trkcase]); gl_LastAnomalyCode = ANOMALY_NONE;//reset anomaly code - added 05/13/23 //move forward one 'skosh' RunBothMotorsMsec(true, 500, MOTOR_SPEED_QTR, MOTOR_SPEED_QTR); delay(100); //may be needed for distance sensors to catch up UpdateAllDistances(); delay(100); UpdateAllDistances(); delay(100); gl_pSerPort->printf("%lu: gl_Left/RightCenterCm = %2.1f/%2.1f, Left/RightSteerVal = %2.2f/%2.2f\n", (unsigned long)gl_ElapsedRunMillisec, gl_LeftCenterCm, gl_RightCenterCm, gl_LeftSteeringVal, gl_RightSteeringVal); if (trkcase == TRACKING_LEFT) { if (gl_LeftCenterCm < MAX_TRACKING_DISTANCE_CM) { //excess steerval anomaly occurred with tracking side dist < MAX_TRACKING_DISTANCE_CM //so this is the 'ChooseBestSide' case on left side newside = ChooseBetterTrackingSide(); if (newside == TRACKING_LEFT) { gl_pSerPort->printf("HandleExcessSteervalCase(%s): ChooseBetterTrackingSide() returned %s\n", WallTrackStrArray[newside]); TrackLeftWallOffset(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd, WALL_OFFSET_TGTDIST_CM); } else if (newside == TRACKING_RIGHT) { gl_pSerPort->printf("HandleExcessSteervalCase(%s): ChooseBetterTrackingSide() returned %s\n", WallTrackStrArray[newside]); TrackRightWallOffset(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd, WALL_OFFSET_TGTDIST_CM); } else //newside == TRACKING_NEITHER - should never happen { gl_pSerPort->printf("HandleExcessSteervalCase(%s): ChooseBetterTrackingSide() returned %s! Should never happen - quitting!\n", WallTrackStrArray[newside]); while (true) { CheckForUserInput(); delay(100); } } } else if (gl_LeftCenterCm >= MAX_TRACKING_DISTANCE_CM) { //excess steerval anomaly occurred with tracking side dist >= MAX_TRACKING_DISTANCE_CM //so this is 'open corner' or 'open doorway case on left side. if (gl_RightCenterCm >= MAX_TRACKING_DISTANCE_CM) { //'open corner' case gl_pSerPort->printf("HandleExcessSteervalCase(%s) open corner block\n", WallTrackStrArray[trkcase]); SpinTurn(trkcase == TRACKING_LEFT, 90); RunBothMotorsMsec(true, 1000); UpdateAllEnvironmentParameters(); gl_pSerPort->printf("HandleExcessSteervalCase) open corner block: gl_Left/RightCenterCm = %2.1f/%2.1f, Left/RightSteerVal = %2.2f/%2.2f\n", gl_LeftCenterCm, gl_RightCenterCm, gl_LeftSteeringVal, gl_RightSteeringVal); TrackLeftWallOffset(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd, WALL_OFFSET_TGTDIST_CM); } else if (gl_RightCenterCm < MAX_TRACKING_DISTANCE_CM) { //'open doorway' case gl_pSerPort->printf("HandleExcessSteervalCase(%s) open doorway block\n", WallTrackStrArray[trkcase]); SpinTurn(!trkcase == TRACKING_LEFT, 90); MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM); SpinTurn(trkcase == TRACKING_LEFT, 90); TrackRightWallOffset(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd, WALL_OFFSET_TGTDIST_CM); } else { //should never happen! gl_pSerPort->printf("HandleExcessSteervalCase(%s) else block - should never execute!\n", WallTrackStrArray[trkcase]); } } } else if (trkcase == TRACKING_RIGHT) { if (gl_RightCenterCm < MAX_TRACKING_DISTANCE_CM) { //excess steerval anomaly occurred with tracking side dist < MAX_TRACKING_DISTANCE_CM //so this is the 'ChooseBestSide' case on left side newside = ChooseBetterTrackingSide(); } else if (gl_RightCenterCm >= MAX_TRACKING_DISTANCE_CM) { //excess steerval anomaly occurred with tracking side dist >= MAX_TRACKING_DISTANCE_CM //so this is 'open corner' or 'open doorway case on left side. if (gl_LeftCenterCm >= MAX_TRACKING_DISTANCE_CM) { //'open corner' case gl_pSerPort->printf("HandleExcessSteervalCase(%s) open corner block\n", WallTrackStrArray[trkcase]); SpinTurn(trkcase == TRACKING_LEFT, 90); RunBothMotorsMsec(true, 1000); } else if (gl_LeftCenterCm < MAX_TRACKING_DISTANCE_CM) { //'open doorway' case gl_pSerPort->printf("HandleExcessSteervalCase(%s) open doorway block\n", WallTrackStrArray[trkcase]); } else { //should never happen! gl_pSerPort->printf("HandleExcessSteervalCase(%s) else block - should never execute!\n", WallTrackStrArray[trkcase]); } } } else { //!OK, excess steerval anomaly occurred with trkcase == TRACKING_NEITHER - shouldn't ever happen gl_pSerPort->printf("%lu: gl_Left/RightCenterCm = %2.1f/%2.1f, Left/RightSteerVal = %2.2f/%2.2f\n", (unsigned long)gl_ElapsedRunMillisec, gl_LeftCenterCm, gl_RightCenterCm, gl_LeftSteeringVal, gl_RightSteeringVal); gl_pSerPort->printf("HandleExcessSteervalCase(TRACKING_NEITHER) occurred - shouldn't have happened! - quitting"); while (true) { CheckForUserInput(); delay(100); } } return result; } |
25 September 2023 Update: Open Corner/Doorway Success!
After quite a bit of yelling, cursing, coding and re-coding, I now believe that WallE3, my autonomous wall-following robot has truly gained the ability to correctly handle ‘open doorway’ and ‘open corner situations. Here’s a short video showing the results of a ‘field test’:
Salient points from the video:
- 0:12: WallE3 encounters an open doorway to the left, with a trackable wall (kitchen counter) to the right. WallE3 stops and transfers to the right wall.
- 0:29: WallE3 encounters an ‘Excess Steerval’ situation, but doesn’t know which wall is better. It utilizes the ‘ChooseBetterTrackingSide()’ function to determine that the left side wall is a better bet, and transfers to it for further progress.
- 0:49: WallE3 encounters an ‘open corner’ situation, where both left and right side distances are > MAX_TRACKING_DISTANCE_CM. Since the last-tracked wall was on the left side, WallE3 turns that way and continues to track around the corner.
- 0:57: WallE3 encounters another ‘open doorway’ situation, and properly transfers to the right wall for onward tracking.
- 1:09: WallE3 encounters another open doorway on the left (non-tracked) side, but because it is tracking the right wall, this is simply ignored.
- 1:14: WallE3 encounters another ‘open doorway’ situation, and properly transfers to the left wall for onward tracking.
- 1:28: WallE3 encounters a WALL_OFFSET_DIST_AHEAD anomaly and quits, because there is no handling code for this yet.
This is by far the ‘cleanest’ run WallE3 has made to date, and gives me some hope for the future. The handling code for the WALL_OFFSET_DIST_AHEAD anomaly has already been written and tested in earlier versions of the code – I just haven’t ported it into the new ‘HandleExcessSteervalCase()’ architecture.
26 September 2023 Update:
Looking at the ‘HandleAnomalousConditions() function in ‘WallE3_Complete_V4.ino’, I see the following:
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 |
void HandleAnomalousConditions(WallTrackingCases trackcase) { //Purpose: Handle anomalous conditions //Inputs: // Call from TrackRightWallOffset() or TrackLeftWallOffset() if an anomalous condition is detected //Outputs: // Handles the error and returns to tracking //Notes: // 03/28/22 added section to handle 'approaching wall' anomaly // 07/30/23 added 'excessive steerval section gl_pSerPort->printf("In HandleAnomalousConditions(%s)\n", WallTrackStrArray[trackcase]); if (gl_bStuckAhead) { #ifndef NO_STUCK //03/14/23 bugfix gl_LastAnomalyCode = ANOMALY_STUCK_AHEAD; //added 03/14/23 gl_pSerPort->printf("ANOMALY_STUCK_AHEAD case detected\n"); ExecuteStuckRecoveryManeuver(trackcase); #endif // } else if (gl_bStuckBehind) { #ifndef NO_STUCK //03/14/23 bugfix gl_LastAnomalyCode = ANOMALY_STUCK_BEHIND; //added 03/14/23 gl_pSerPort->printf("ANOMALY_STUCK_BEHIND case detected\n"); ExecuteRearObstacleRecoveryManeuver(trackcase); #endif // } else if (gl_bObstacleAhead) { gl_LastAnomalyCode = ANOMALY_OBSTACLE_AHEAD; //added 03/14/23 gl_pSerPort->printf("ANOMALY_OBSTACLE_AHEAD case detected with tracking case %s\n", TrkStrArray[trackcase]); StopBothMotors(); delay(1000); //delay just for visual indication that we got here if (trackcase == TRACKING_LEFT) { BackupAndTurn90Deg(false, true, MOTOR_SPEED_FULL);//tracking left wall, so backup & then forward CW turn } else if (trackcase == TRACKING_RIGHT) { BackupAndTurn90Deg(true, true, MOTOR_SPEED_FULL);//tracking right wall, so backup & then forward CCW turn } else { gl_pSerPort->printf("in HandleAnomalousConditions(%s,)", TrkStrArray[trackcase]); } } else if (gl_bObstacleBehind) { gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND; //added 03/14/23 gl_pSerPort->printf("ANOMALY_OBSTACLE_BEHIND case detected\n"); ExecuteRearObstacleRecoveryManeuver(trackcase); } else if (gl_bWallOffsetDistAhead) { gl_LastAnomalyCode = ANOMALY_WALL_OFFSET_DIST_AHEAD; //added 03/14/23 gl_pSerPort->printf("ANOMALY_WALL_OFFSET_DIST_AHEAD case detected with tracking case %s\n", TrkStrArray[trackcase]); StopBothMotors(); SpinTurn(trackcase == TRACKING_RIGHT, 90); } else if (gl_bDeadBattery) { gl_LastAnomalyCode = ANOMALY_DEAD_BATTERY; //added 03/14/23 gl_pSerPort->printf("ANOMALY_DEAD_BATTERY case detected\n"); YellForHelp(); //doesn't return unless charger is connected } else if (gl_bChgConnect) { gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED; } else if (gl_bOpenCorner) { gl_LastAnomalyCode = ANOMALY_OPEN_CORNER; //added 03/14/23 gl_pSerPort->printf("ANOMALY_OPEN_CORNER case detected\n"); //05/13/23 added to enable OPEN_CORNER processing StopBothMotors(); delay(500); //just for visual indication purposes RunBothMotorsMsec(true, 500, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //straight ahead a bit SpinTurn(trackcase == TRACKING_LEFT, 90.f, 45.f); //90deg turn toward last tracked wall RunBothMotorsMsec(true, 500, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //07/27/23 added } else if (gl_bOpenDoorway) { gl_LastAnomalyCode = ANOMALY_OPEN_DOORWAY; //added 03/14/23 gl_pSerPort->printf("ANOMALY_OPEN_DOORWAY case detected\n"); StopBothMotors(); delay(1500);//for visual indication only //added 06/17/23 to get robot oriented parallel to 'other' wall if (trackcase == TRACKING_LEFT) { RotateToParallelOrientation(TRACKING_RIGHT); } else { RotateToParallelOrientation(TRACKING_LEFT); } delay(1500);//for visual indication only MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW); //move slowly straight ahead //uint16_t openDoorwayCount = 0; //for (uint16_t i = 0; i < 5; i++) //{ // delay(200); // if (IsOpenDoorway(trackcase)) // { // openDoorwayCount++; // } //} // //if (openDoorwayCount >= 3) //{ // gl_LastAnomalyCode = ANOMALY_OPEN_DOORWAY; //added 03/14/23 // gl_pSerPort->printf("ANOMALY_OPEN_DOORWAY case detected\n"); //} } else if (gl_bTrackingWrongWall) { gl_LastAnomalyCode = ANOMALY_TRACKING_WRONG_WALL; //added 03/14/23 gl_pSerPort->printf("ANOMALY_TRACKING_WRONG_WALL case detected\n"); gl_pSerPort->printf("gl_WrongWallCounter = %d\n"); } else if (gl_bExcessiveSteerVal) { gl_LastAnomalyCode = ANOMALY_EXCESS_STEER_VAL; //added 03/14/23 gl_pSerPort->printf("ANOMALY_EXCESS_STEER_VAL case detected\n"); } else { gl_LastAnomalyCode = ANOMALY_NONE; } } |
For the ‘ANOMALY_OBSTACLE_AHEAD’ case, the response was to call ‘BackupAndTurn90Deg(trkcase == TRACKING_RIGHT, true, MOTOR_SPEED_FULL), so it backed up and turned right (CW) if it was tracking the left wall, and left (CCW) if it was tracking the right wall. The ‘BackupAndTurn90Deg()’ function turns all the LEDs ON, calls ‘MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM)’ to move backward, turns all the LEDs OFF, and then calls ‘SpinTurn()’ to do the 90deg turn. Note that this procedure now ignores the ‘uint16_t motor_speed’ parameter in the sig to ‘BackupAndTurn90Deg().
The ‘BackupAndTurn90Deg()’ function is called in only two places; the above HandleAnomalousConditions() block, and in ‘IRHomeToChgStn()’. Both calls specify a speed to use, which ‘BackupAndTurn90Deg()’ ignores because the ‘backup’ part of ‘BackupAndTurn90Deg()’ is controlled by a PID and is limited to MOTOR_SPD_QTR.
So, I have revised BackupAndTurn90Deg() to remove the speed parameter and the front/back boolean from the signature, and ported the code for ANOMALY_OBSTACLE_AHEAD, ANOMALY_STUCK_AHEAD, ANOMALY_STUCK_BEHIND and ANOMALY_WALL_OFFSET_DIST_AHEAD cases from WallE3_Complete_V4
28 September 2023 Update:
After porting the remaining anomaly code handlers to WallE3_Complete_V5, I made another ‘field test’. This time WallE3 travelled all the way down the hallway as before, but this time instead of stopping with an unhandled anomaly condition, the robot backed up from the door, turned around a few times, and then butted its head into the left-side wall – oops! It then backed up and did it again! If I hadn’t turned it off, I suspect it would have continued doing that forever. Here’s the video showing the action:
The telemetry from the above run is 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 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 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 |
gl_pSerPort now points to active Serial (USB or Wixel) 7950: Starting setup() for WallE3_Complete_V5.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 10572 0.069 0.076 MPU6050 Ready at 10.57 Sec with delta = -0.007 Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x20 Teensy available at 11672 with gl_bVL53L0X_TeensyReady = 1. Waiting for Teensy setup() to finish 11675: got 1 from VL53L0X Teensy Teensy setup() finished at 11777 mSec VL53L0X Teensy Ready at 11780 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 = 4 100 -0.3333 11942: Initializing IR Beam Total Value Averaging Array...Done 11945: Initializing IR Beam Steering Value Averaging Array...Done Battery Voltage = 7.86 14153: End of setup(): Elapsed run time set to 0 0.0: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.86 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 0.4: gl_Left/RightCenterCm = 24.2/116.7, Left/RightSteerVal = 0.03/1.00 In HandleAnomalousConditions(NEITHER) with last anomaly code = NONE In HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE- 0.5: gl_Left/RightCenterCm = 24.3/119.3, Left/RightSteerVal = 0.00/1.00 gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset() TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 0.5 Just after CaptureWallOffset(TRACKING_LEFT, 24.3) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.6 23.7 118.9 0.4 23.9 24.5 -0.06 434 48 63713 12363 127 7 NONE LEFT 0.7 24.3 114.0 4.0 24.0 24.3 -0.03 412 48 56822 12212 126 23 NONE LEFT 0.8 25.4 107.1 9.1 25.3 24.5 0.08 388 52 51266 12223 83 66 NONE LEFT 0.9 26.5 106.6 11.9 26.8 25.6 0.12 371 57 47063 12360 69 80 NONE LEFT 1.0 27.6 106.5 12.2 27.8 27.1 0.07 361 61 44085 12607 71 78 NONE LEFT 1.1 29.2 104.3 11.5 29.5 27.9 0.17 363 64 42122 12949 29 120 NONE LEFT 1.2 30.1 107.3 8.5 30.4 30.1 0.03 363 68 41042 13360 63 86 NONE LEFT 1.3 30.4 126.1 5.7 30.9 30.0 0.09 374 73 40608 13802 43 106 NONE LEFT 1.4 30.9 141.8 3.2 31.3 30.6 0.07 386 76 40696 14263 50 99 NONE LEFT 1.5 30.7 146.8 0.6 31.6 30.9 0.07 390 79 41160 14723 50 99 NONE LEFT 1.6 31.1 151.1 -1.6 31.0 31.0 0.00 398 82 41884 15142 68 81 NONE LEFT 1.7 31.5 150.0 -2.0 31.3 31.9 -0.08 395 86 42906 15487 94 55 NONE LEFT 1.8 31.6 150.1 -1.1 32.1 32.2 -0.04 390 91 44064 15725 82 68 NONE LEFT 1.9 32.1 151.1 -0.6 32.1 32.2 -0.01 382 98 45194 15813 63 86 NONE LEFT 2.0 32.6 149.6 -1.7 32.7 32.3 0.04 376 102 46028 15716 47 102 NONE LEFT 2.2 32.9 149.1 -3.6 33.3 32.8 0.05 380 107 46288 15412 44 105 NONE LEFT 2.2 33.2 149.0 -4.8 32.5 33.0 -0.05 391 111 46080 15167 69 80 NONE LEFT 2.3 33.1 149.2 -5.7 32.8 32.9 -0.01 1000 115 49862 14855 58 91 NONE LEFT 2.4 33.2 148.8 -6.2 32.6 33.1 -0.05 1000 120 53425 14472 70 79 NONE LEFT 2.5 32.6 148.9 -6.5 32.8 33.0 -0.02 1000 124 56768 14014 68 81 NONE LEFT 2.7 33.0 150.0 -7.4 31.9 32.8 -0.04 624 130 58736 12861 68 81 NONE LEFT 2.8 32.4 150.1 -7.8 32.1 32.5 -0.04 545 135 57193 12158 74 75 NONE LEFT 2.9 32.5 149.7 -7.9 31.8 32.5 -0.07 523 140 55351 11367 84 65 NONE LEFT 2.9 32.0 150.1 -7.5 31.4 32.1 -0.07 839 144 55146 10484 85 64 NONE LEFT 3.0 32.0 150.1 -6.9 31.3 31.9 -0.06 503 149 52694 9507 82 67 NONE LEFT 3.1 31.1 150.5 -6.7 31.0 32.0 -0.10 1000 154 54482 8430 108 41 NONE LEFT 3.2 30.9 150.8 -6.0 30.9 31.5 -0.06 1000 159 55957 7254 96 53 NONE LEFT 3.3 31.1 150.7 -4.6 30.6 31.3 -0.07 674 163 53004 5972 92 57 NONE LEFT 3.5 31.4 150.9 -2.8 31.0 30.8 0.02 319 169 46000 3067 61 88 NONE LEFT 3.5 31.4 149.8 -2.8 31.4 31.2 0.02 318 171 44053 1427 61 89 NONE LEFT 3.6 31.3 149.6 -3.4 31.0 31.7 -0.07 297 180 45202 1584 92 57 NONE LEFT 3.7 31.5 147.7 -3.1 31.3 31.1 0.02 307 182 46326 1715 61 88 NONE LEFT 3.8 31.5 148.0 -3.3 31.4 31.2 0.02 307 186 46831 1776 61 89 NONE LEFT 4.0 31.6 147.0 -3.7 31.3 31.7 -0.04 297 193 47856 1884 81 69 NONE LEFT 4.1 31.5 146.4 -3.9 31.7 31.6 0.01 294 198 48332 1945 65 84 NONE LEFT 4.1 32.4 144.4 -3.3 31.5 31.5 0.00 659 202 48885 2007 61 88 NONE LEFT 4.2 31.9 145.8 -3.2 31.4 32.1 -0.07 277 204 49438 2063 90 59 NONE LEFT 4.3 31.7 146.1 -2.9 31.4 31.8 -0.04 283 210 50403 2176 82 67 NONE LEFT 4.4 31.8 145.4 -2.3 31.6 31.7 -0.01 271 215 51548 2262 71 78 NONE LEFT 4.5 32.1 145.6 -1.9 32.0 32.1 -0.01 264 215 52759 2308 64 85 NONE LEFT 4.6 32.1 146.0 -1.7 32.5 32.3 0.02 264 221 54674 2340 54 95 NONE LEFT 4.7 32.4 144.7 -1.3 32.7 32.5 0.02 256 224 57598 2243 54 95 NONE LEFT 4.8 32.9 145.9 -1.7 32.6 32.6 0.00 245 229 59786 2098 61 89 NONE LEFT 4.9 33.0 145.7 -2.0 32.8 32.8 -0.06 244 236 61865 1907 75 75 NONE LEFT 5.0 33.0 146.9 -2.2 33.2 32.7 0.05 233 242 63856 1700 38 111 NONE LEFT 5.1 33.3 148.7 -3.2 33.2 32.8 0.04 237 246 65766 1459 41 108 NONE LEFT 5.2 33.4 148.3 -3.8 33.4 33.4 0.00 592 253 59478 1392 53 96 NONE LEFT 5.3 33.4 148.7 -5.0 33.3 33.4 -0.01 987 258 59172 1338 57 92 NONE LEFT 5.4 33.6 148.4 -6.2 32.6 33.3 -0.07 987 263 58867 1293 77 72 NONE LEFT 5.6 33.2 148.5 -7.1 32.5 33.5 -0.10 632 272 58938 1223 94 55 NONE LEFT 5.7 32.9 149.2 -7.3 32.6 33.2 -0.06 965 276 64727 1201 82 67 NONE LEFT 5.8 32.9 149.3 -6.4 32.2 32.7 -0.05 840 281 68070 1185 78 71 NONE LEFT 5.9 32.9 148.4 -5.7 32.1 32.8 -0.07 666 285 65764 1182 85 64 NONE LEFT 5.9 32.5 149.0 -5.1 32.5 33.0 -0.05 930 288 70798 1182 78 71 NONE LEFT 6.0 32.5 149.5 -4.7 32.4 32.4 0.00 950 290 69699 1187 62 88 NONE LEFT 6.1 32.3 150.0 -4.6 31.9 32.2 -0.03 894 294 67481 1204 70 79 NONE LEFT 6.2 32.7 149.6 -4.6 32.3 32.2 0.01 946 300 71663 1233 58 91 NONE LEFT 6.4 32.3 149.9 -5.3 32.3 32.5 -0.05 937 305 80489 1294 77 72 NONE LEFT 6.5 32.6 147.7 -5.8 32.4 32.4 0.00 915 309 84257 1323 62 88 NONE LEFT 6.6 32.5 142.8 -6.3 32.2 32.3 -0.01 917 316 87750 1362 64 85 NONE LEFT 6.7 32.5 138.1 -6.9 31.9 32.4 -0.05 184 319 88813 1413 77 72 NONE LEFT 6.7 32.1 130.5 -7.4 31.9 32.5 -0.06 747 328 89631 1476 81 68 NONE LEFT 6.8 32.1 125.5 -7.3 31.5 31.9 -0.04 301 326 89674 1520 75 74 NONE LEFT 6.9 31.7 132.9 -6.7 31.6 32.0 -0.04 1000 334 94202 1584 81 68 NONE LEFT 7.0 32.0 137.6 -6.3 31.5 31.9 -0.04 887 325 96320 1612 75 74 NONE LEFT 7.2 31.2 132.2 -5.8 30.7 31.7 -0.10 1000 335 101415 1710 109 40 NONE LEFT 7.3 31.1 127.7 -4.3 30.6 30.7 -0.01 877 320 103431 1717 73 76 NONE LEFT 7.4 30.5 122.5 -2.6 29.9 30.7 -0.08 1000 289 106100 1663 101 48 NONE LEFT 7.5 30.6 129.4 -1.5 30.2 30.9 -0.07 1000 214 109803 1650 99 50 NONE LEFT 7.5 31.1 122.6 -0.4 30.6 30.8 -0.02 880 185 110169 1718 76 73 NONE LEFT 7.6 30.8 129.1 1.1 30.9 30.8 0.01 107 148 117409 2246 71 78 NONE LEFT 7.7 31.4 131.4 1.1 33.1 31.0 0.21 100 137 126986 3204 8 127 NONE LEFT 7.8 32.9 129.9 -1.2 37.5 31.7 0.58 94 153 136117 4038 0 127 NONE LEFT 8.0 36.7 104.9 -10.8 40.6 34.8 0.58 684 366 135835 4301 0 127 NONE LEFT 8.0 36.7 98.3 -15.1 40.2 34.8 0.54 266 370 135579 4574 0 127 NONE LEFT 8.1 36.9 87.0 -21.0 39.4 35.3 0.41 229 380 135837 5175 0 127 NONE LEFT 8.2 36.9 77.1 -31.9 36.6 36.8 -0.02 213 337 137171 5887 35 114 NONE LEFT 8.4 39.7 68.1 -45.2 36.6 42.3 -0.57 995 256 137869 5922 127 0 NONE LEFT 8.5 41.0 66.3 -45.1 37.0 42.8 -0.58 948 123 138188 6331 127 0 NONE LEFT 8.6 36.8 66.1 -42.1 35.1 38.0 -0.29 960 240 140895 6343 127 0 NONE LEFT 8.7 36.4 66.5 -36.8 36.4 35.7 0.07 350 273 138880 6342 17 127 NONE LEFT 8.7 36.8 67.5 -32.8 37.5 36.3 0.12 276 300 137299 6362 0 127 NONE LEFT 8.8 39.9 67.9 -33.6 40.7 38.5 0.13 243 343 137333 6529 0 127 NONE LEFT 9.0 40.8 64.5 -39.9 41.5 40.4 0.11 320 9 134024 9192 0 127 NONE LEFT 9.0 41.3 63.0 -46.1 41.0 40.9 0.01 805 34 135206 10173 0 127 NONE LEFT 9.1 41.3 60.5 -52.6 41.1 40.7 0.04 808 11 133518 11300 0 127 NONE LEFT 9.2 41.2 59.7 -61.2 41.0 41.7 -0.07 326 18 127785 13288 21 127 NONE LEFT 9.3 43.3 62.0 -70.7 42.2 44.0 -0.18 129 127 117633 13655 61 89 NONE LEFT 9.4 46.9 216.2 -75.7 43.8 46.6 -0.28 92 134 111885 13112 99 50 NONE LEFT 9.5 44.5 206.3 -75.0 42.3 45.1 -0.28 92 138 92281 12012 108 41 NONE LEFT 9.6 43.3 285.5 -73.4 40.0 43.6 -0.36 93 130 68243 12026 127 0 NONE LEFT 9.7 40.2 243.2 -69.8 37.3 40.3 -0.30 101 117 68410 12177 127 4 NONE LEFT 9.8 36.9 251.9 -62.5 34.3 36.4 -0.21 206 116 67232 12445 120 29 NONE LEFT 9.9 33.9 208.2 -57.3 32.4 33.5 -0.11 234 237 54684 11032 99 50 NONE LEFT 10.0 31.9 172.4 -52.1 31.7 31.8 -0.01 513 287 56173 9734 73 76 NONE LEFT 10.2 30.3 155.7 -47.9 30.2 30.4 0.00 758 34 64899 8709 76 73 NONE LEFT 10.2 30.8 157.5 -47.2 30.2 30.6 -0.04 794 315 70057 8325 88 61 NONE LEFT 10.3 30.7 162.1 -46.8 30.9 30.6 0.03 806 3 75182 8086 65 84 NONE LEFT 10.4 31.5 165.8 -47.2 30.8 30.9 -0.01 821 56 80518 7624 71 78 NONE LEFT 10.6 31.1 192.9 -48.1 31.4 31.6 -0.10 758 318 71507 8426 108 41 NONE LEFT 10.7 31.4 176.1 -47.4 31.6 31.4 0.02 765 330 67117 8921 63 86 NONE LEFT 10.8 31.7 198.9 -46.7 31.2 31.6 -0.04 800 315 72027 9168 80 69 NONE LEFT 10.9 31.9 187.2 -47.0 31.9 31.9 0.00 796 342 76687 9452 68 81 NONE LEFT 10.9 32.4 200.4 -47.4 32.1 31.5 0.06 779 342 80696 9582 41 108 NONE LEFT 11.0 32.9 201.0 -48.1 31.9 32.3 -0.04 757 334 84125 9515 73 77 NONE LEFT 11.1 32.5 219.9 -48.8 32.3 32.5 -0.02 699 338 86562 9728 68 81 NONE LEFT 11.2 32.8 252.7 -49.0 32.2 32.2 0.03 680 340 86754 10168 50 99 NONE LEFT 11.3 32.3 271.1 -49.3 32.1 32.5 -0.04 671 348 84621 10232 73 76 NONE LEFT 11.4 32.2 271.8 -49.7 32.3 32.9 -0.06 653 341 88357 10134 81 68 NONE LEFT 11.6 32.5 279.7 -49.8 32.0 32.7 -0.07 665 344 90961 10879 84 65 NONE LEFT 11.6 32.8 267.9 -49.7 32.2 32.4 -0.02 658 346 91313 11185 69 81 NONE LEFT 11.7 32.4 273.4 -49.7 32.4 32.3 0.03 650 344 91215 11442 51 98 NONE LEFT 11.8 32.8 280.2 -50.2 32.1 32.4 -0.03 640 338 89672 11845 71 78 NONE LEFT 11.9 32.7 289.8 -50.9 32.3 32.3 0.00 621 331 86753 11983 62 87 NONE LEFT 12.0 32.8 288.6 -51.0 32.4 31.9 0.05 619 355 84819 12137 44 105 NONE LEFT 12.1 32.6 296.0 -52.4 31.9 32.3 -0.04 605 355 80109 12336 75 75 NONE LEFT 12.2 32.4 297.2 -52.9 31.4 32.4 -0.07 582 360 74412 12450 84 65 NONE LEFT 12.3 32.2 282.4 -52.4 31.2 31.9 -0.07 585 339 67909 12264 85 64 NONE LEFT 12.4 32.2 90.8 -51.6 31.3 31.2 0.01 582 362 60625 11991 59 90 NONE LEFT 12.5 31.5 82.0 -51.1 30.9 31.5 -0.06 595 334 56747 11714 87 62 NONE LEFT 12.6 30.7 76.4 -50.6 29.8 30.7 -0.09 582 330 48443 10973 107 42 NONE LEFT 12.7 31.8 75.5 -48.9 70.7 30.3 1.00 604 179 39605 10248 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 12.8: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.83 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 13.2: gl_Left/RightCenterCm = 256.7/75.7, Left/RightSteerVal = -0.61/0.10 In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase(LEFT) 14111: gl_Left/RightCenterCm = 255.1/78.5, Left/RightSteerVal = 0.99/0.13 HandleExcessSteervalCase(LEFT) open doorway block In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 79.35/78.11/0.12 Calling SpinTurn() for angle = 7.1 In SpinTurn(CW, 7.09, 15.00) with PID = (0.7,0.3,0.0) RTPO: ending front/rear/steer avgs = 78.90/78.45/0.05 RTPO: GetWallOrientDeg(0.05) returned ending orientation of 2.57 deg TrackRightWallOffset(350.0, 0.0, 20.0, 30) called TrackRightWallOffset: Start tracking offset of 30cm at 18.7 Calling CaptureWallOffset(TRACKING_RIGHT, 78.8) Hdg Dist In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0) After SpinTurn, Hdg/Dist = 38.8/73 MTFD: at start, tgt = 30cm, curr_dist = 72, front/rear var = 83325.0/13328.0 Msec Fdist TgtD err Ival Out Speed FVar RVar 35066 73 30 -43.0 -4.3 -60.2 -60 80956 11897 35118 73 30 -43.0 -8.6 -73.1 -73 80644 11273 35168 71 30 -41.0 -12.7 -74.6 -74 80711 10697 35218 71 30 -41.0 -16.8 -78.3 -75 81109 10234 35268 70 30 -40.0 -20.8 -81.0 -75 81836 10438 35320 69 30 -39.0 -24.7 -83.4 -75 82875 10741 35370 67 30 -37.0 -28.4 -84.3 -75 84224 10597 35420 65 30 -35.0 -31.9 -84.8 -75 85862 10263 35470 63 30 -33.0 -35.2 -85.1 -75 87767 9962 35520 59 30 -29.0 -38.1 -82.4 -75 89951 10369 35570 56 30 -26.0 -40.7 -80.3 -75 92372 10859 35620 54 30 -24.0 -43.1 -79.5 -75 94988 10720 35670 51 30 -21.0 -45.2 -77.3 -75 97788 11303 35720 48 30 -18.0 -47.0 -74.6 -74 100746 11373 35770 46 30 -16.0 -48.6 -73.0 -73 103815 11347 35820 43 30 -13.0 -49.9 -70.0 -69 106982 11983 35870 40 30 -10.0 -50.9 -66.5 -66 110214 12661 35920 38 30 -8.0 -51.7 -64.1 -64 113463 13248 35970 36 30 -6.0 -52.3 -61.7 -61 116696 13699 36020 35 30 -5.0 -52.8 -60.5 -60 119860 13714 36070 34 30 -4.0 -53.2 -59.4 -59 122922 13734 36120 31 30 -1.0 -53.3 -55.4 -55 125875 13781 MTFD: Stopped with front dist = 30 In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0) In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 22.20/21.65/0.06 End of CaptureWallOffset Just after CaptureWallOffset(TRACKING_RIGHT, 21.0) Sec LCen RCen Deg RF RR RStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 26.2 206.4 21.1 -47.2 22.2 21.5 0.07 699 169 128841 13792 38 111 NONE RIGHT 26.3 203.1 21.6 -48.9 22.0 22.1 -0.01 704 164 130351 13811 10 127 NONE RIGHT 26.3 209.5 21.4 -52.0 21.7 21.7 0.00 660 141 131292 13868 11 127 NONE RIGHT 26.4 211.2 21.7 -56.4 22.4 21.2 0.12 574 15 131453 14489 51 98 NONE RIGHT 26.5 200.5 22.5 -61.5 23.8 21.4 0.24 517 3 131220 15156 87 63 NONE RIGHT 26.5 196.9 24.0 -65.3 23.4 22.8 0.06 449 8 130679 15746 56 94 NONE RIGHT 26.6 122.0 22.8 -66.2 24.7 23.2 0.15 319 198 130124 15700 63 86 NONE RIGHT 26.6 132.0 23.3 -67.0 25.2 22.5 0.27 310 201 129368 15637 97 52 NONE RIGHT 26.7 146.5 24.8 -67.2 25.6 23.0 0.26 282 204 128456 15556 109 40 NONE RIGHT 26.8 118.5 25.5 -66.7 27.1 24.6 0.25 282 207 127226 15452 113 36 NONE RIGHT 26.8 181.9 26.0 -65.9 27.7 25.5 0.22 303 208 125570 15319 110 39 NONE RIGHT 26.9 136.3 27.1 -64.4 28.5 26.3 0.22 319 49 123499 15451 116 33 NONE RIGHT 27.0 135.0 27.2 -62.1 29.7 26.8 0.29 453 19 121131 15669 127 16 NONE RIGHT 27.0 163.9 28.1 -60.3 29.8 26.7 0.31 473 207 118486 15406 127 8 NONE RIGHT 27.1 209.2 28.7 -57.8 29.9 28.3 0.16 482 7 115506 15541 111 38 NONE RIGHT 27.1 181.4 29.7 -54.4 30.3 28.6 0.17 512 7 112299 15562 119 30 NONE RIGHT 27.2 98.6 28.8 -51.2 30.1 29.0 0.11 561 17 109045 15407 94 55 NONE RIGHT 27.2 83.5 29.8 -48.0 29.9 30.3 -0.04 631 168 106074 14813 56 93 NONE RIGHT 27.3 77.1 30.0 -46.0 30.2 29.9 0.03 659 172 103019 14147 83 66 NONE RIGHT 27.4 76.0 29.4 -45.7 29.9 30.1 -0.02 670 19 99666 13653 62 87 NONE RIGHT 27.5 76.7 28.6 -46.3 29.5 29.6 -0.01 666 5 95794 13077 57 92 NONE RIGHT 27.5 76.2 28.7 -47.1 29.2 29.3 -0.01 653 188 91255 12136 57 92 NONE RIGHT 27.6 76.5 28.7 -47.9 29.0 28.8 0.02 632 186 85920 11096 67 82 NONE RIGHT 27.7 77.3 28.7 -48.5 28.8 29.1 -0.03 629 183 79982 9942 51 98 NONE RIGHT 27.7 77.7 28.8 -49.5 28.5 28.3 0.02 609 190 73197 8699 67 83 NONE RIGHT 27.8 78.0 28.1 -50.3 29.1 29.2 -0.01 592 165 65565 7274 58 91 NONE RIGHT 27.9 78.3 28.3 -51.1 29.1 28.4 0.07 582 197 66030 5816 83 66 NONE RIGHT 27.9 79.3 28.6 -51.5 28.6 28.4 0.02 570 204 66168 5953 69 81 NONE RIGHT 28.0 80.0 28.3 -51.7 29.1 27.8 0.13 565 206 66066 6114 97 52 NONE RIGHT 28.0 82.3 28.7 -51.2 29.1 28.7 0.04 547 203 65603 6270 76 73 NONE RIGHT 28.1 85.5 28.6 -50.3 29.6 28.6 0.10 564 205 65090 6432 94 55 NONE RIGHT 28.2 90.4 28.5 -49.8 29.2 28.7 0.04 574 211 64460 6628 76 73 NONE RIGHT 28.2 97.1 28.2 -49.2 29.2 29.2 0.00 579 209 63658 6593 61 88 NONE RIGHT 28.3 108.2 28.4 -48.8 29.5 29.2 0.03 581 206 62645 6490 70 79 NONE RIGHT 28.3 115.2 28.6 -49.0 28.5 28.9 -0.04 579 201 61380 6585 48 101 NONE RIGHT 28.4 117.0 28.5 -49.0 28.6 29.2 -0.06 580 198 59886 6688 40 109 NONE RIGHT 28.4 125.9 28.1 -49.5 29.0 28.7 -0.01 562 203 58020 6802 56 93 NONE RIGHT 28.5 132.2 28.1 -50.3 28.9 28.5 0.04 560 198 55892 6662 74 76 NONE RIGHT 28.6 134.6 27.9 -50.7 28.8 28.7 0.01 543 198 53442 6468 58 91 NONE RIGHT 28.6 140.2 28.1 -50.4 28.6 28.4 0.02 533 202 50723 6563 67 82 NONE RIGHT 28.7 148.3 28.0 -50.2 28.4 28.3 0.01 542 208 47804 6342 64 85 NONE RIGHT 28.7 165.8 28.1 -50.2 28.1 28.5 -0.04 539 159 44640 6312 48 102 NONE RIGHT 28.8 173.9 28.0 -50.4 28.4 28.2 0.02 547 202 41264 6387 66 83 NONE RIGHT 28.8 179.1 28.3 -50.7 29.0 29.1 -0.01 538 211 37610 6133 58 91 NONE RIGHT 28.9 184.6 28.1 -50.9 29.3 28.6 0.07 530 211 33700 5827 83 66 NONE RIGHT 29.0 231.0 28.2 -51.1 29.6 28.3 0.13 525 207 29554 5551 98 51 NONE RIGHT 29.0 248.6 28.6 -50.8 29.1 28.6 0.04 522 35 25188 5583 76 73 NONE RIGHT 29.1 270.5 28.1 -50.9 29.3 28.9 0.04 521 205 20613 5636 75 75 NONE RIGHT 29.1 268.6 28.5 -51.1 28.9 29.1 -0.02 505 225 15818 5734 55 94 NONE RIGHT 29.2 267.1 28.6 -51.0 29.2 28.9 0.03 511 218 10797 5810 70 79 NONE RIGHT 29.2 268.2 28.5 -51.3 29.0 28.3 0.07 503 214 10278 5871 84 65 NONE RIGHT 29.3 270.5 28.3 -51.2 29.3 29.0 0.03 495 217 9704 5939 72 77 NONE RIGHT 29.3 267.4 28.4 -51.1 29.5 28.9 0.06 501 221 9364 6008 81 68 NONE RIGHT 29.4 265.6 28.7 -51.1 29.4 28.8 0.07 497 210 9331 5626 85 64 NONE RIGHT 29.4 263.9 28.7 -51.0 29.1 29.2 0.02 496 201 9345 5123 69 81 NONE RIGHT 29.5 258.9 28.7 -50.6 29.6 29.2 0.04 501 192 9243 4606 74 75 NONE RIGHT 29.5 191.5 28.9 -50.5 29.0 29.1 -0.01 500 29 8397 4984 58 91 NONE RIGHT 29.6 91.3 28.5 -50.3 29.8 29.3 0.05 496 37 7452 5293 77 72 NONE RIGHT 29.6 78.8 28.9 -50.3 29.1 29.1 0.00 493 151 6215 5264 62 88 NONE RIGHT 29.7 76.8 28.6 -50.5 29.4 29.0 0.04 485 211 4959 5269 74 75 NONE RIGHT 29.7 75.4 28.6 -50.4 29.3 29.0 0.03 484 211 3884 5275 71 78 NONE RIGHT 29.8 75.5 28.8 -50.4 29.5 29.0 0.05 486 162 2929 5003 78 71 NONE RIGHT 29.8 75.9 29.1 -50.2 29.6 29.0 0.06 485 209 2828 4589 88 61 NONE RIGHT 29.9 75.8 28.9 -50.0 29.7 29.3 0.04 483 214 2800 4601 75 74 NONE RIGHT 29.9 76.0 28.9 -49.6 32.5 29.6 0.29 489 214 2783 4087 127 12 NONE RIGHT 30.0 76.5 28.7 -49.5 29.5 29.3 0.02 490 17 2824 4015 72 77 NONE RIGHT 30.0 76.5 28.9 -48.4 29.3 29.2 0.01 491 106 2883 3608 64 85 NONE RIGHT 30.1 76.2 28.6 -47.9 29.5 29.6 -0.03 493 219 2789 3643 51 98 NONE RIGHT 30.1 76.5 29.1 -47.7 29.2 29.3 -0.01 489 222 2570 3682 63 86 NONE RIGHT 30.2 76.9 28.4 -48.1 28.9 29.2 -0.03 490 226 2270 3206 51 98 NONE RIGHT 30.2 76.5 28.3 -48.1 28.6 29.3 -0.07 490 223 1966 2593 37 112 NONE RIGHT 30.3 76.9 28.3 -48.5 28.9 29.2 -0.03 480 225 1722 2622 49 100 NONE RIGHT 30.3 76.9 28.1 -49.0 28.6 28.9 -0.03 471 220 1570 2642 50 99 NONE RIGHT 30.4 77.1 28.7 -49.7 28.7 28.7 0.00 460 226 1436 2668 60 89 NONE RIGHT 30.4 76.9 28.4 -50.3 28.7 28.8 -0.01 455 220 1371 2687 57 92 NONE RIGHT 30.5 76.9 28.6 -50.9 29.0 28.8 0.02 455 206 1344 2679 67 82 NONE RIGHT 30.5 77.4 28.4 -51.4 30.8 28.9 0.19 441 201 1367 2681 110 39 NONE RIGHT 30.6 77.1 28.0 -51.7 35.1 28.5 0.66 428 194 1446 2677 127 0 NONE RIGHT RightSteer = 1.0, result = TRUE 30.6 77.1 30.2 -51.5 44.9 28.5 1.00 420 214 1548 2684 127 0 EXCESS_STEER_VAL RIGHT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 30.6: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.82 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 31.1: gl_Left/RightCenterCm = 76.9/49.9, Left/RightSteerVal = -0.02/0.36 In HandleAnomalousConditions(RIGHT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(RIGHT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase(RIGHT) 31980: gl_Left/RightCenterCm = 79.1/94.1, Left/RightSteerVal = -0.04/-1.00 In ChooseBetterTrackingSide with avg hdg = -50.0 TurnToHdgDeg: currHdg = -48.0, tgtHdg = -50.0, retries = 0 In SpinTurn(CCW, 1.99, 45.00) with PID = (0.7,0.3,0.0) In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -58.1, left/right dists = 275.0/85.7, numleftright = 0/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -69.0, left/right dists = 244.0/305.3, numleftright = 0/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -79.4, left/right dists = 639.0/308.0, numleftright = 0/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -89.9, left/right dists = 117.0/246.2, numleftright = 0/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -99.5, left/right dists = 98.0/223.8, numleftright = 1/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -109.5, left/right dists = 86.0/198.1, numleftright = 2/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -120.9, left/right dists = 77.0/182.2, numleftright = 3/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -130.8, left/right dists = 75.0/179.8, numleftright = 4/1 In SpinTurn(CCW, 10.00, 45.00) with PID = (0.7,0.3,0.0) In ChooseBetterTrackingSide: hdg = -140.3, left/right dists = 75.0/312.2, numleftright = 5/1 TurnToHdgDeg: currHdg = -141.8, tgtHdg = -50.0, retries = 0 In SpinTurn(CW, 91.87, 45.00) with PID = (0.7,0.3,0.0) ChooseBetterTrackingSide 'left' block with numleft/rightwallsegments = 5/1 HandleExcessSteervalCase(LEFT): ChooseBetterTrackingSide() returned TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 37.1 Calling CaptureWallOffset(TRACKING_LEFT, 171.5) Hdg Dist In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0) After SpinTurn, Hdg/Dist = -138.2/77 MTFD: at start, tgt = 30cm, curr_dist = 76, front/rear var = 83325.0/13328.0 Msec Fdist TgtD err Ival Out Speed FVar RVar 53487 78 30 -48.0 -4.8 -67.2 -67 80770 12344 53537 77 30 -47.0 -9.5 -80.2 -75 80390 11937 53587 76 30 -46.0 -14.1 -83.3 -75 80372 11583 53637 75 30 -45.0 -18.6 -86.3 -75 80702 11283 53687 74 30 -44.0 -23.0 -89.2 -75 81363 11044 53740 72 30 -42.0 -27.2 -90.6 -75 82352 11163 53790 72 30 -42.0 -31.4 -94.4 -75 83618 11422 53840 69 30 -39.0 -35.3 -94.4 -75 85191 11287 53890 67 30 -37.0 -39.0 -94.9 -75 87033 11181 53940 65 30 -35.0 -42.5 -95.4 -75 89120 11112 53990 61 30 -31.0 -45.6 -92.9 -75 91463 11262 54040 60 30 -30.0 -48.6 -93.8 -75 93987 11753 54090 57 30 -27.0 -51.3 -92.4 -75 96697 11816 54140 53 30 -23.0 -53.6 -88.9 -75 99585 12377 54190 51 30 -21.0 -55.7 -87.6 -75 102586 12962 54240 49 30 -19.0 -57.6 -86.5 -75 105671 13339 54290 47 30 -17.0 -59.3 -85.2 -75 108810 13421 54340 44 30 -14.0 -60.7 -82.3 -75 111986 13481 54389 40 30 -10.0 -61.7 -77.5 -75 115182 13541 54439 37 30 -7.0 -62.4 -73.5 -73 118345 13604 54489 36 30 -6.0 -63.0 -72.2 -72 121408 13668 54540 34 30 -4.0 -63.4 -69.8 -69 124351 13725 54590 31 30 -1.0 -63.5 -65.6 -65 127150 13779 MTFD: Stopped with front dist = 30 In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0) In RotateToParallelOrientation(LEFT) End of CaptureWallOffset Just after CaptureWallOffset(TRACKING_LEFT, 22.0) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 44.6 21.8 149.9 -48.4 22.3 22.6 -0.03 243 178 128836 13783 127 2 NONE LEFT 44.8 22.2 155.1 -41.6 23.1 22.3 0.07 371 173 128086 13769 108 41 NONE LEFT 44.9 23.0 133.7 -34.3 22.5 23.2 -0.07 479 74 126472 14129 127 4 NONE LEFT 45.0 23.8 117.9 -28.8 27.2 22.1 0.51 501 44 124338 14618 0 127 NONE LEFT 45.0 25.2 115.8 -28.7 25.3 25.3 0.00 509 51 122972 14777 103 46 NONE LEFT 45.2 27.0 118.3 -29.9 27.6 25.6 0.20 499 40 117004 14984 38 111 NONE LEFT 45.3 28.1 121.2 -32.7 29.5 27.1 0.24 481 54 111183 14582 19 127 NONE LEFT 45.4 29.6 155.4 -37.2 30.1 28.5 0.16 471 37 103524 13873 31 118 NONE LEFT 45.4 30.1 157.3 -42.6 30.2 28.9 0.21 397 20 93338 12895 15 127 NONE LEFT 45.6 30.8 155.2 -48.9 30.5 29.9 0.06 279 151 80291 10985 54 96 NONE LEFT 45.6 30.8 223.6 -52.9 30.5 30.7 -0.05 218 213 64167 8973 89 60 NONE LEFT 45.8 31.4 275.5 -55.5 30.7 31.7 -0.10 251 229 44258 6748 107 42 NONE LEFT 45.9 31.6 296.4 -55.3 30.3 31.4 -0.11 248 233 32385 4182 112 37 NONE LEFT 46.0 31.1 300.2 -54.4 30.3 30.6 -0.03 251 238 31501 4899 78 71 NONE LEFT 46.1 30.0 316.9 -53.5 29.5 30.5 -0.10 227 243 30505 5589 109 40 NONE LEFT 46.2 30.0 293.2 -52.2 29.2 30.0 -0.07 206 239 29434 6182 99 50 NONE LEFT 46.2 29.8 296.8 -50.5 29.2 29.8 -0.05 183 240 28370 6380 100 49 NONE LEFT 46.3 29.6 265.3 -48.5 29.5 29.6 -0.01 189 189 27202 6629 84 65 NONE LEFT 46.4 29.9 290.0 -47.7 29.6 29.9 -0.03 199 222 25851 6824 92 57 NONE LEFT 46.5 30.1 291.0 -47.5 29.8 29.9 -0.01 208 188 24304 6646 77 72 NONE LEFT 46.6 30.3 273.8 -47.6 30.6 30.4 0.02 195 184 22643 5972 68 81 NONE LEFT 46.7 30.2 305.8 -47.5 30.1 30.5 -0.04 203 95 20787 5827 87 62 NONE LEFT 46.8 31.0 323.1 -47.0 30.8 31.0 -0.02 206 164 18749 5798 76 73 NONE LEFT 46.9 31.4 308.4 -46.3 31.0 30.9 0.00 231 176 16385 5814 67 82 NONE LEFT 47.0 31.2 267.6 -46.7 31.2 31.0 0.02 222 147 13840 5804 61 89 NONE LEFT 47.1 31.3 276.1 -47.0 31.7 31.3 0.06 205 18 14038 6228 47 102 NONE LEFT 47.2 32.1 311.8 -47.6 32.1 31.7 0.04 185 91 14296 6553 48 102 NONE LEFT 47.3 32.6 298.8 -48.4 31.9 32.0 -0.01 157 93 13702 6493 63 86 NONE LEFT 47.4 32.6 325.6 -49.1 32.3 32.4 -0.01 139 91 12352 6240 64 85 NONE LEFT 47.5 32.7 310.4 -49.6 35.2 32.1 0.31 129 95 10528 5943 0 127 NONE LEFT 47.6 35.7 320.4 -51.2 61.3 32.0 1.00 113 98 8520 5577 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 47.7: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.80 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 48.1: gl_Left/RightCenterCm = 58.0/150.0, Left/RightSteerVal = 1.00/-1.00 In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase(LEFT) 48945: gl_Left/RightCenterCm = 120.4/132.5, Left/RightSteerVal = 1.00/0.98 HandleExcessSteervalCase(LEFT) open corner block In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0) HandleExcessSteervalCase(LEFT) open corner block after turn and move: gl_Left/RightCenterCm = 26.0/118.3, Left/RightSteerVal = -0.31/1.00 TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 52.1 Just after CaptureWallOffset(TRACKING_LEFT, 26.0) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 52.2 23.8 94.6 -160.4 21.4 24.9 -0.22 63 172 3805 4752 127 0 NONE LEFT 52.3 21.6 150.5 -157.3 22.1 23.7 -0.29 68 201 3047 3944 127 0 NONE LEFT 52.4 19.7 125.9 -153.2 17.8 21.6 -0.38 83 192 3234 3907 127 0 NONE LEFT 52.5 18.0 133.9 -146.5 17.5 18.8 -0.13 162 204 3209 3894 127 0 NONE LEFT 52.6 16.4 112.8 -138.9 16.2 17.4 -0.12 524 163 5506 3750 127 0 NONE LEFT 52.7 16.1 101.6 -131.7 15.9 16.4 -0.06 534 153 10203 3555 127 0 NONE LEFT 52.8 16.9 91.2 -121.9 17.5 17.2 0.03 330 213 12562 3392 127 0 NONE LEFT 52.9 17.9 85.7 -118.0 18.2 17.9 0.03 239 47 12580 3509 127 0 NONE LEFT 53.0 21.5 84.2 -109.3 22.2 20.2 0.20 162 35 12594 3817 76 73 NONE LEFT 53.1 23.5 87.4 -106.8 24.1 20.9 0.32 139 30 12704 3901 31 118 NONE LEFT 53.2 23.8 85.3 -110.2 26.1 22.8 0.33 152 31 12782 4038 34 115 NONE LEFT 53.3 25.1 83.2 -116.3 27.3 24.0 0.33 178 43 12800 3926 22 127 NONE LEFT 53.4 25.3 87.9 -122.3 26.8 24.6 0.23 227 50 12820 3802 42 107 NONE LEFT 53.5 25.7 93.1 -125.7 27.0 25.0 0.20 279 58 12968 3755 53 96 NONE LEFT 53.6 26.4 107.6 -129.3 26.6 25.9 0.09 475 92 15302 3663 71 78 NONE LEFT 53.7 26.3 120.7 -130.3 26.4 26.0 0.04 493 224 18378 3867 88 62 NONE LEFT 53.8 27.1 120.8 -130.1 26.9 26.9 0.00 486 189 22584 4538 95 54 NONE LEFT 53.9 27.7 125.2 -129.3 28.1 27.7 0.04 479 183 25034 5121 81 68 NONE LEFT 54.0 28.6 127.9 -129.5 28.7 27.7 0.10 471 97 27019 4904 54 95 NONE LEFT 54.1 29.0 132.6 -130.9 28.9 28.4 0.05 475 81 28650 4762 64 85 NONE LEFT 54.2 28.8 137.1 -132.2 29.5 29.1 0.03 477 90 29756 4770 78 71 NONE LEFT 54.3 29.3 146.8 -132.6 30.0 29.7 0.03 471 100 30318 4752 71 78 NONE LEFT 54.4 30.0 149.9 -132.9 30.3 29.5 0.08 474 125 30415 4718 48 101 NONE LEFT 54.5 30.2 115.1 -134.3 30.2 29.7 0.05 465 84 29932 4886 56 93 NONE LEFT 54.6 30.7 65.7 -136.9 30.4 30.3 0.01 456 239 28260 5068 71 78 NONE LEFT 54.7 30.9 60.9 -138.1 30.8 29.9 0.09 450 241 25703 5510 45 104 NONE LEFT 54.8 29.9 62.3 -139.4 30.6 30.5 0.01 447 152 22686 5528 78 71 NONE LEFT 54.9 29.9 62.6 -139.6 29.9 30.1 -0.02 439 176 19446 5483 88 61 NONE LEFT 55.0 30.3 62.8 -139.2 29.6 30.0 -0.04 434 219 16998 5445 89 60 NONE LEFT 55.1 29.9 62.1 -138.6 30.2 30.0 -0.02 431 180 16233 5556 87 62 NONE LEFT 55.2 29.6 64.0 -138.8 29.3 29.5 -0.02 425 109 15472 5544 89 61 NONE LEFT 55.3 29.9 64.2 -138.5 29.3 29.6 -0.03 423 220 15127 5722 92 57 NONE LEFT 55.4 29.9 64.0 -137.6 29.3 29.2 0.01 418 45 14002 5551 79 70 NONE LEFT 55.5 29.1 63.1 -136.9 29.1 29.1 0.00 413 16 11963 5510 81 68 NONE LEFT 55.6 29.7 62.9 -136.7 29.5 29.4 0.01 407 129 9062 5365 78 71 NONE LEFT 55.7 29.2 63.1 -137.0 29.3 29.3 0.00 403 126 6116 4924 81 68 NONE LEFT 55.8 29.9 64.3 -137.6 29.1 29.1 0.00 402 99 3414 4569 83 67 NONE LEFT 55.9 28.7 64.4 -137.2 28.0 29.1 -0.11 395 117 1451 4390 127 17 NONE LEFT 56.0 28.5 63.1 -135.7 29.2 28.7 0.05 395 140 964 4202 74 75 NONE LEFT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL 56.1: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.81 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 56.5: gl_Left/RightCenterCm = 197.1/62.9, Left/RightSteerVal = 0.08/-0.28 In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase(LEFT) 57338: gl_Left/RightCenterCm = 200.6/60.8, Left/RightSteerVal = -0.10/0.04 HandleExcessSteervalCase(LEFT) open doorway block In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 61.94/63.08/-0.11 Calling SpinTurn() for angle = -6.5 In SpinTurn(CCW, 6.51, 15.00) with PID = (0.7,0.3,0.0) RTPO: ending front/rear/steer avgs = 62.48/62.37/0.01 RTPO: GetWallOrientDeg(0.01) returned ending orientation of 0.63 deg TrackRightWallOffset(350.0, 0.0, 20.0, 30) called TrackRightWallOffset: Start tracking offset of 30cm at 62.0 Calling CaptureWallOffset(TRACKING_RIGHT, 62.5) Hdg Dist In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0) After SpinTurn, Hdg/Dist = -53.0/59 MTFD: at start, tgt = 30cm, curr_dist = 59, front/rear var = 83325.0/13328.0 Msec Fdist TgtD err Ival Out Speed FVar RVar 78394 59 30 -29.0 -2.9 -40.6 -40 81517 11793 78444 60 30 -30.0 -5.9 -50.7 -50 81429 11076 78494 60 30 -30.0 -8.9 -53.9 -53 81686 10392 78544 59 30 -29.0 -11.8 -55.5 -55 82289 9744 78594 58 30 -28.0 -14.6 -56.8 -56 83221 9145 78644 56 30 -26.0 -17.2 -56.6 -56 84481 9366 78694 54 30 -24.0 -19.6 -56.0 -56 86047 9703 78744 53 30 -23.0 -21.9 -56.6 -56 87882 10039 78794 52 30 -22.0 -24.1 -57.3 -57 89963 9705 78844 50 30 -20.0 -26.1 -56.5 -56 92285 9331 78894 49 30 -19.0 -28.0 -56.7 -56 94805 8987 78944 50 30 -20.0 -30.0 -59.8 -59 97465 8671 78994 47 30 -17.0 -31.7 -57.8 -57 100306 8385 79044 47 30 -17.0 -33.4 -58.9 -58 103251 8130 79094 43 30 -13.0 -34.7 -55.0 -55 106336 7953 79144 41 30 -11.0 -35.8 -52.7 -52 109499 7764 79194 39 30 -9.0 -36.7 -50.6 -50 112708 7706 79246 37 30 -7.0 -37.4 -48.3 -48 115930 7731 79294 36 30 -6.0 -38.0 -47.2 -47 119116 8428 79344 36 30 -6.0 -38.6 -47.6 -47 122215 8963 79394 35 30 -5.0 -39.1 -46.8 -46 125210 8907 79447 33 30 -3.0 -39.4 -44.3 -44 128079 9230 79497 32 30 -2.0 -39.6 -42.8 -42 130769 9990 79547 31 30 -1.0 -39.7 -41.4 -41 133242 9989 MTFD: Stopped with front dist = 31 In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0) In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 28.01/27.51/0.05 End of CaptureWallOffset Just after CaptureWallOffset(TRACKING_RIGHT, 27.5) Sec LCen RCen Deg RF RR RStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 69.6 227.0 27.5 -145.3 27.5 27.6 -0.01 355 261 134006 10010 51 98 NONE RIGHT 69.7 223.9 27.2 -145.5 28.6 27.5 0.11 354 204 133486 10014 83 66 NONE RIGHT 69.7 223.7 27.7 -146.0 27.9 27.5 0.04 358 309 132791 10159 69 81 NONE RIGHT 69.8 227.0 27.4 -145.1 28.1 26.8 0.13 353 54 131901 10734 91 58 NONE RIGHT 69.8 228.4 27.8 -144.7 28.5 27.3 0.08 351 3 130786 11668 82 67 NONE RIGHT 69.9 222.4 28.4 -143.6 28.7 27.8 0.07 353 3 129415 12552 85 64 NONE RIGHT 69.9 226.2 28.4 -142.8 29.0 28.0 0.10 350 27 127761 13186 95 54 NONE RIGHT 70.0 218.5 28.6 -142.0 28.6 28.2 0.04 344 222 125792 13142 76 73 NONE RIGHT 70.0 216.1 28.2 -141.1 29.0 28.7 0.03 342 112 123474 13244 71 78 NONE RIGHT 70.1 227.4 28.7 -140.8 29.1 28.3 0.08 340 7 120772 13900 88 62 NONE RIGHT 70.1 230.3 28.8 -140.6 29.5 28.8 0.07 343 3 117655 14495 85 64 NONE RIGHT 70.2 226.7 29.0 -140.2 29.0 28.3 0.07 340 2 114085 15005 92 57 NONE RIGHT 70.2 227.7 29.3 -139.6 29.3 29.2 0.01 334 4 110021 15400 72 77 NONE RIGHT 70.3 226.0 29.0 -139.0 29.5 28.9 0.06 333 3 105427 15693 88 62 NONE RIGHT 70.3 226.2 29.3 -138.7 29.3 28.9 0.04 332 5 100265 15856 82 67 NONE RIGHT 70.4 229.7 28.6 -138.4 29.6 29.5 0.01 328 8 94490 15876 65 84 NONE RIGHT 70.4 228.5 29.2 -138.1 29.5 29.3 0.02 325 5 88059 15789 74 75 NONE RIGHT 70.5 226.9 29.1 -138.3 29.8 29.4 0.04 325 205 80936 15182 81 68 NONE RIGHT 70.5 225.1 29.8 -138.3 29.9 30.0 -0.01 322 238 73069 14594 65 84 NONE RIGHT 70.6 227.3 29.5 -138.3 29.8 29.7 0.01 319 272 64413 14072 71 78 NONE RIGHT 70.6 221.1 29.9 -138.5 29.9 30.0 -0.02 318 238 54926 13334 61 88 NONE RIGHT 70.7 224.2 29.8 -138.8 29.8 29.4 0.04 318 265 44566 12627 80 69 NONE RIGHT 70.7 214.8 29.8 -139.1 30.3 30.1 0.02 316 266 33278 11848 75 74 NONE RIGHT 70.8 188.4 29.5 -139.0 29.4 30.5 -0.11 311 271 20996 11003 25 124 NONE RIGHT 70.8 120.4 29.4 -139.0 30.2 29.9 0.03 309 270 20956 10063 75 74 NONE RIGHT 70.9 94.7 29.3 -139.9 30.2 29.9 0.03 304 256 20845 10254 78 71 NONE RIGHT 70.9 85.2 30.1 -140.2 30.5 29.7 0.08 303 166 20681 10205 101 48 NONE RIGHT 71.0 70.4 29.5 -140.3 30.5 30.0 0.05 297 243 20450 10354 86 63 NONE RIGHT 71.0 67.6 29.7 -139.7 31.0 30.1 0.09 299 291 20179 10739 98 51 NONE RIGHT 71.1 65.5 29.8 -139.3 30.2 30.1 0.01 294 240 19839 10866 73 76 NONE RIGHT 71.1 63.7 29.8 -138.5 30.8 31.1 -0.03 293 231 19442 10986 58 91 NONE RIGHT 71.2 62.6 30.1 -138.4 30.3 30.8 -0.05 291 174 18981 10682 57 92 NONE RIGHT 71.2 64.3 30.0 -138.7 30.4 30.7 -0.03 290 286 18463 10670 64 85 NONE RIGHT 71.3 62.1 29.8 -139.2 30.8 31.0 -0.02 286 144 17884 10296 61 88 NONE RIGHT 71.3 63.3 29.9 -139.7 30.6 30.4 0.03 283 185 17245 10315 77 72 NONE RIGHT 71.4 61.9 30.4 -140.0 30.3 30.6 0.00 282 218 16550 10348 75 74 NONE RIGHT 71.4 63.7 30.9 -140.0 31.1 31.0 0.01 279 296 15807 10690 78 71 NONE RIGHT 71.5 62.8 30.5 -139.8 31.4 31.2 0.02 275 202 15010 10672 81 68 NONE RIGHT 71.5 62.5 30.3 -139.5 31.5 30.8 0.07 273 244 14159 10765 98 51 NONE RIGHT 71.6 64.4 31.1 -139.3 30.7 31.1 -0.04 272 199 13248 10750 69 80 NONE RIGHT 71.6 61.4 30.5 -138.6 31.1 30.9 0.02 269 268 12269 10985 81 68 NONE RIGHT 71.7 64.3 30.7 -138.4 31.0 32.3 -0.13 265 189 11232 10968 25 124 NONE RIGHT 71.7 62.6 30.5 -138.1 31.0 30.9 0.01 262 219 10136 11035 75 74 NONE RIGHT 71.8 63.7 30.7 -138.7 31.0 31.0 0.00 260 240 8987 11147 75 74 NONE RIGHT 71.8 65.8 31.1 -139.2 30.8 30.7 0.01 262 218 7791 10828 84 65 NONE RIGHT 71.9 66.7 30.3 -139.2 31.1 30.9 0.02 257 185 6555 10557 82 67 NONE RIGHT 71.9 69.4 30.7 -138.9 30.8 30.9 -0.01 256 225 5266 10595 72 77 NONE RIGHT 72.0 73.9 30.1 -138.4 31.0 31.8 -0.08 252 211 3928 10493 48 101 NONE RIGHT 72.0 78.9 30.7 -138.2 31.0 31.7 -0.07 249 241 2548 10134 50 99 NONE RIGHT 72.1 85.5 30.4 -138.6 30.4 31.3 -0.03 248 259 1126 10264 63 86 NONE RIGHT 72.1 99.6 30.6 -139.2 30.8 30.9 -0.05 244 218 1147 10157 57 92 NONE RIGHT 72.2 115.3 30.1 -139.9 31.0 31.1 -0.01 243 188 1163 10145 70 79 NONE RIGHT 72.2 153.0 30.3 -140.5 31.0 31.2 -0.02 240 270 1168 9968 68 81 NONE RIGHT 72.3 168.0 30.3 -141.0 31.0 31.3 -0.03 241 248 1170 9755 64 85 NONE RIGHT 72.3 205.6 30.1 -141.2 30.9 31.1 -0.02 235 226 1181 9149 67 82 NONE RIGHT 72.4 214.5 30.8 -141.7 31.5 31.3 0.02 234 170 1178 8480 81 68 NONE RIGHT 72.4 233.5 30.8 -142.0 31.2 31.7 -0.05 232 155 1176 7983 58 91 NONE RIGHT 72.5 236.3 30.9 -142.0 31.6 31.7 -0.01 231 182 1179 7964 70 79 NONE RIGHT 72.5 241.4 31.7 -142.4 31.7 31.6 0.01 228 179 1184 7846 84 65 NONE RIGHT 72.6 221.4 32.1 -142.5 32.3 31.9 0.04 225 186 1189 7155 102 48 NONE RIGHT 72.6 215.4 30.9 -142.2 32.2 31.9 0.03 226 186 1175 6415 86 63 NONE RIGHT 72.7 248.3 31.8 -141.5 32.3 32.2 0.01 221 177 1167 5647 85 64 NONE RIGHT 72.7 248.2 31.9 -141.1 32.2 32.2 0.00 221 150 1164 4915 82 67 NONE RIGHT 72.8 250.2 32.2 -140.7 32.2 32.7 -0.05 217 171 1161 4120 72 77 NONE RIGHT 72.8 244.6 32.5 -140.5 32.9 32.7 0.00 214 200 1157 3288 88 62 NONE RIGHT 72.9 250.1 32.7 -140.4 32.9 32.9 0.00 212 214 1155 2446 89 61 NONE RIGHT 72.9 260.4 32.2 -140.1 33.2 32.8 0.04 209 182 1157 1565 102 47 NONE RIGHT 73.0 251.5 32.6 -139.7 33.0 33.0 0.00 208 212 1150 1562 89 60 NONE RIGHT 73.0 244.7 32.7 -138.9 33.3 33.5 -0.02 204 210 1149 1557 82 67 NONE RIGHT 73.1 248.0 32.5 -138.3 33.5 34.1 -0.06 203 244 1146 1514 68 81 NONE RIGHT 73.1 206.4 33.6 -138.0 34.0 33.7 0.03 201 220 1139 1506 104 45 NONE RIGHT 73.2 231.0 33.2 -137.9 33.5 33.8 -0.03 197 178 1131 1493 86 63 NONE RIGHT 73.2 262.3 33.4 -137.1 33.3 33.9 -0.06 196 205 1117 1443 75 74 NONE RIGHT 73.3 276.7 33.2 -136.6 33.5 34.2 -0.07 194 217 1110 1380 71 78 NONE RIGHT 73.3 216.2 33.1 -136.3 33.4 34.0 -0.06 193 185 1099 1331 74 75 NONE RIGHT 73.4 227.4 33.3 -136.4 33.6 34.6 -0.10 190 185 1095 1306 55 94 NONE RIGHT 73.4 284.6 33.1 -136.5 34.2 34.0 0.02 187 221 1090 1268 100 49 NONE RIGHT 73.5 116.0 33.7 -136.8 33.7 34.4 -0.07 185 221 1093 1249 73 76 NONE RIGHT 73.5 95.7 33.2 -136.5 33.4 33.5 -0.01 183 105 1086 1331 91 58 NONE RIGHT 73.6 75.7 33.8 -136.3 33.3 34.4 -0.11 182 160 1082 1353 52 97 NONE RIGHT 73.6 63.3 32.9 -136.1 33.6 34.4 -0.08 177 210 1081 1340 60 89 NONE RIGHT 73.7 60.9 33.0 -136.4 33.2 34.7 -0.12 177 159 1075 1362 47 102 NONE RIGHT 73.7 59.8 33.1 -137.0 33.2 34.7 -0.15 174 159 1066 1266 37 112 NONE RIGHT 73.8 59.5 32.7 -137.9 33.3 34.1 -0.09 171 189 1064 1201 56 93 NONE RIGHT 73.8 59.6 32.8 -138.9 33.4 33.9 -0.05 169 192 1062 1198 70 79 NONE RIGHT 73.9 59.5 32.9 -139.8 33.1 33.9 -0.05 168 200 1055 1194 71 78 NONE RIGHT 73.9 59.0 33.2 -140.3 33.8 34.1 -0.03 166 186 1048 1021 84 65 NONE RIGHT 74.0 59.0 33.1 -140.7 33.9 33.8 0.01 164 179 1045 1031 98 51 NONE RIGHT 74.0 58.8 33.6 -140.6 33.4 33.6 -0.02 163 201 1039 992 89 60 NONE RIGHT 74.1 58.7 33.2 -140.1 33.7 33.7 0.00 159 400 1035 1781 95 54 NONE RIGHT 74.1 59.3 33.2 -139.6 33.9 33.9 0.00 160 161 1024 1731 96 54 NONE RIGHT 74.2 59.6 33.3 -138.9 34.0 34.6 -0.06 157 176 1020 1740 76 73 NONE RIGHT 74.2 60.8 33.9 -138.1 33.6 34.5 -0.09 155 111 1018 1895 65 84 NONE RIGHT 74.3 61.5 33.2 -137.8 34.0 34.4 -0.04 151 195 1018 1861 81 69 NONE RIGHT 74.3 59.1 33.4 -137.8 34.1 34.7 -0.06 151 195 1005 1853 75 74 NONE RIGHT 74.4 58.4 33.7 -137.6 34.2 34.9 -0.03 146 156 1004 1885 84 65 NONE RIGHT 74.4 60.3 33.7 -137.5 34.2 34.6 -0.04 142 156 1004 1904 82 67 NONE RIGHT 74.5 60.0 33.6 -137.2 34.3 34.9 -0.06 140 210 1008 1903 75 74 NONE RIGHT 74.5 58.9 33.6 -136.9 34.5 34.9 -0.06 138 219 1013 1873 75 75 NONE RIGHT 74.6 58.7 33.1 -136.9 34.3 35.1 -0.06 136 208 1014 1793 75 75 NONE RIGHT 74.6 60.7 34.4 -137.1 34.6 35.3 -0.07 134 198 1019 1782 78 71 NONE RIGHT 74.7 58.8 33.8 -137.2 34.5 35.0 -0.05 133 191 1018 1781 78 71 NONE RIGHT 74.7 59.2 33.3 -137.3 34.3 35.3 -0.10 131 198 1019 1663 55 94 NONE RIGHT 74.8 60.4 33.6 -137.5 34.1 35.4 -0.13 127 196 1017 1602 44 105 NONE RIGHT 74.8 58.8 34.2 -138.1 34.5 34.3 0.02 125 176 1022 1583 106 43 NONE RIGHT 74.9 60.4 33.6 -139.1 34.4 35.2 -0.08 122 176 1027 1579 70 79 NONE RIGHT 74.9 58.5 33.8 -139.1 34.5 34.5 0.00 120 191 1031 1553 94 55 NONE RIGHT 75.0 59.6 33.9 -139.0 34.3 35.3 -0.10 118 182 1031 1552 56 93 NONE RIGHT 75.0 58.7 34.3 -138.7 35.2 35.3 -0.01 116 182 1032 1551 96 53 NONE RIGHT 75.1 59.8 33.6 -138.8 34.8 34.9 -0.03 114 174 1034 1556 86 63 NONE RIGHT 75.1 58.3 34.0 -138.4 34.3 35.3 -0.05 112 206 1029 1560 85 64 NONE RIGHT 75.2 60.1 34.0 -138.2 35.1 35.3 -0.10 111 195 1027 1556 62 87 NONE RIGHT 75.2 58.3 34.6 -138.1 34.5 35.5 -0.10 107 216 1024 1530 61 89 NONE RIGHT 75.3 59.5 33.9 -138.5 34.3 36.2 -0.19 106 194 1022 1520 18 127 NONE RIGHT 75.3 58.1 33.5 -139.4 34.2 36.0 -0.18 102 170 1026 1530 18 127 NONE RIGHT 75.4 57.7 34.8 -141.2 33.5 35.4 -0.19 103 189 1022 1522 22 127 NONE RIGHT 75.4 59.3 33.0 -143.3 35.1 35.0 0.01 99 155 1024 1547 95 54 NONE RIGHT 75.5 58.2 33.5 -145.7 43.7 34.5 0.92 98 180 1020 1542 127 0 NONE RIGHT RightSteer = 1.0, result = TRUE 75.5 58.3 159.9 -146.7 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 76.0: gl_Left/RightCenterCm = 56.9/184.2, Left/RightSteerVal = 0.07/0.00 In HandleAnomalousConditions(RIGHT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(RIGHT) ANOMALY_EXCESS_STEER_VAL CASE- Top of HandleExcessSteervalCase(RIGHT) 76805: gl_Left/RightCenterCm = 55.4/196.3, Left/RightSteerVal = -0.09/0.02 HandleExcessSteervalCase(RIGHT) open doorway block In RotateToParallelOrientation(LEFT) In SpinTurn(CW, 6.06, 15.00) with PID = (0.7,0.3,0.0) TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 81.4 Calling CaptureWallOffset(TRACKING_LEFT, 54.5) Hdg Dist In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0) After SpinTurn, Hdg/Dist = 130.6/48 MTFD: at start, tgt = 30cm, curr_dist = 47, front/rear var = 83325.0/13328.0 Msec Fdist TgtD err Ival Out Speed FVar RVar 97786 48 30 -18.0 -1.8 -25.2 -25 82013 11857 97836 48 30 -18.0 -3.6 -30.6 -30 82137 11165 97886 47 30 -17.0 -5.3 -31.0 -31 82624 10481 97936 49 30 -19.0 -7.2 -35.3 -35 83402 9825 97986 47 30 -17.0 -8.9 -34.8 -34 84525 9197 98036 48 30 -18.0 -10.7 -37.5 -37 85921 8569 98086 47 30 -17.0 -12.4 -38.1 -38 87603 7970 98135 45 30 -15.0 -13.9 -36.8 -36 89570 7399 98186 45 30 -15.0 -15.4 -37.9 -37 91762 6857 98236 45 30 -15.0 -16.9 -39.4 -39 94156 6345 98286 44 30 -14.0 -18.3 -39.5 -39 96747 5875 98336 44 30 -14.0 -19.7 -40.7 -40 99490 5427 98386 43 30 -13.0 -21.0 -40.7 -40 102377 5008 98435 40 30 -10.0 -22.0 -37.6 -37 105414 4622 98486 41 30 -11.0 -23.1 -39.4 -39 108505 4265 98536 40 30 -10.0 -24.1 -39.3 -39 111653 3935 98586 38 30 -8.0 -24.9 -37.3 -37 114844 3633 98636 38 30 -8.0 -25.7 -37.7 -37 118013 3358 98686 38 30 -8.0 -26.5 -38.5 -38 121129 3133 98736 36 30 -6.0 -27.1 -36.5 -36 124188 2913 98786 35 30 -5.0 -27.6 -35.3 -35 127141 2725 98836 34 30 -4.0 -28.0 -34.2 -34 129951 2555 98886 33 30 -3.0 -28.3 -33.0 -32 132580 2412 98936 33 30 -3.0 -28.6 -33.1 -33 134976 2289 98986 32 30 -2.0 -28.8 -32.0 -31 137115 2183 99036 32 30 -2.0 -29.0 -32.0 -31 138943 2096 99086 31 30 -1.0 -29.1 -30.8 -30 140433 2027 MTFD: Stopped with front dist = 31 In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0) In RotateToParallelOrientation(LEFT) End of CaptureWallOffset Just after CaptureWallOffset(TRACKING_LEFT, 31.2) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 89.2 31.1 175.0 -139.3 30.9 31.4 -0.05 59 204 141704 2140 87 62 NONE LEFT 89.3 30.6 171.2 -138.1 30.9 31.6 -0.07 56 64 140Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 90.4: gl_Left/RightCenterCm = 31.8/162.3, Left/RightSteerVal = -0.03/-1.00 In HandleAnomalousConditions(LEFT) with last anomaly code = WALL_OFFSET_DIST_AHEAD ANOMALY_OBSTACLE_AHEAD case detected with tracking case LEFT In BackupAndTurn90Deg(CW)with front distance = 18 MTFD: at start, tgt = 30cm, curr_dist = 18, front/rear var = 83325.0/13328.0 Msec Fdist TgtD err Ival Out Speed FVar RVar MTFD: Stopped with front dist = 19 In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0) BackupAndTurn90Deg finished 93.6: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.79 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 94.1: gl_Left/RightCenterCm = 23.4/186.2, Left/RightSteerVal = 0.22/1.00 In HandleAnomalousConditions(LEFT) with last anomaly code = WALL_OFFSET_DIST_AHEAD ANOMALY_OBSTACLE_AHEAD case detected with tracking case LEFT In BackupAndTurn90Deg(CW)with front distance = 224 MTFD: at start, tgt = 30cm, curr_dist = 219, front/rear var = 8BackupAndTurn90Deg finished 97.3: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.79 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 97. |
As shown in the video and telemetry, everything goes swimmingly until about 89sec (90sec & change in the video) where the robot reports a WALL_OFFSET_DIST_AHEAD anomaly. This is expected, but WALLE3 doesn’t back up as far as it should, and then after doing the expected 90º CW turn, instead of starting to track the new left wall, it does another 180º CW turn and runs headlong into the old left wall (the left wall of the hallway, facing the closed door). Regarding the backup maneuver, it appears it started at 18cm and stopped at 19cm, well short of the expected 30cm.
From the code in HandleAnomalousConditions(), The ANOMALY_OBSTACLE_AHEAD case is shown below:
1 2 3 4 5 6 7 8 9 10 11 12 13 |
case ANOMALY_OBSTACLE_AHEAD: {//09/28/23 added braces for scoping, collapsibility //09/26/23 ported from WallE3_Complete_V4.ino gl_LastAnomalyCode = ANOMALY_OBSTACLE_AHEAD; //added 03/14/23 gl_pSerPort->printf("ANOMALY_OBSTACLE_AHEAD case detected with tracking case %s\n", WallTrackStrArray[trackcase]); StopBothMotors(); delay(1000); //delay just for visual indication that we got here //09/26/23 backup, turn 90 away from currently tracked wall, go forward, turn back parallel BackupAndTurn90Deg(trackcase == TRACKING_RIGHT); RunBothMotorsMsec(true); SpinTurn(trackcase == TRACKING_LEFT, 90); } |
So BackupAndTurn90Deg(false) is called which should result in a backup to 30cm, then a 90º CW turn. Looking at BackupAndTurn90Deg(false), we see
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 |
bool BackupAndTurn90Deg(bool b_ccw) { //Purpose: Backup 1 second and then turn 90 degrees //Provenance: G. Frank Paynter 06/27/2020 //Inputs: // bIsCCW = bool denoting direction for the turn // bIsFwd = bool denoting fwd/bkwd direction for the turn // motor_speed = int denoting motor speed to use for the maneuver //Outputs: // Robot backs up 1 sec, then turns 90 deg in the specified direction //Plan: // Step1: Back up for 1 sec // Step2: Turn 90 deg in specified direction //Notes: // 06/27/20 Replaces BackupAndTurn in all Tracking modes // 10/13/20 rev to terminate backup at Wall Offset Distance // 02/15/22 rev to use MoveToDesiredFrontDistCm(OFFSET) // 08/28/22 rev to return bool result from MoveToDesiredFrontDistCm //Step1: Back up (or go forward) to WALL_OFFSET_TGTDIST_CM //gl_pSerPort->printf("In BackupAndTurn90Deg(%s, %s, %d)with front distance = %d\n", // b_ccw ? "CCW" : "CW", b_fwd ? "FWD" : "REV", motor_speed, GetFrontDistCm()); gl_pSerPort->printf("In BackupAndTurn90Deg(%s)with front distance = %d\n", b_ccw ? "CCW" : "CW", GetFrontDistCm()); EnableAllRearLEDs(true);; //turn all rear panel LEDs ON bool result = MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);//08/28/22 rev to return bool result EnableAllRearLEDs(false);; //turn all rear panel LEDs OFF //Step2: Turn 90 deg fwd or backward in specified direction SpinTurn(b_ccw, 90); //06/27/20 experiment gl_pSerPort->printf("BackupAndTurn90Deg finished\n"); return result; // 08/28/22 rev to return bool result from MoveToDesiredFrontDistCm } |
So BackupAndTurn90Deg() calls MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM) which should back up to 30cm.
In MoveToDesiredFrontDistCm() we have:
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 |
bool MoveToDesiredFrontDistCm(uint16_t offsetCm) { //Purpose: Move forward or backward to a specified front offset distance // Inputs: // offsetCm - int denoting desired forward distance offset // Kp,Ki,Kd set at top of program (lines 85-87) // Outputs: // Robot moves forward or backward to the desired offset, under PID control //Notes: // 04/25/21 the Kp/Ki/Kd values are much lower than prescribed by Z-N theory // but I like the slower, more deliberate action. // 03/06/22 rev for Teensy 3.5 and PIDCalcs(). // 05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' positioning means // using a minus sign here for motor speed. float lastError = 0; float lastInput = 0; float lastIval = 0; float lastDerror = 0; //DEBUG!! //gl_pSerPort->printf("MoveToDesiredFrontDistCm(%d): Kp/Ki/Kd= %2.1f/%2.1f/%2.1f\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.1f/%2.1f\n", offsetCm, GetFrontDistCm(), gl_Frontvar, gl_Rearvar); gl_pSerPort->printf("Msec\tFdist\tTgtD\terr\tIval\tOut\tSpeed\tFVar\tRVar\n"); UpdateAllEnvironmentParameters(); //AnomalyCode errcode = CheckForAnomalies();c/o 06/11/23 - chks now done in HandleAnomalousConditions() MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate //08/21/23 replaced '&& !gl_bStuckAhead && !gl_bStuckBehind' with '&& gl_LastAnomalyCode == ANOMALY_NONE' while (abs(gl_FrontCm - offsetCm) > 1 && gl_LastAnomalyCode == ANOMALY_NONE) { CheckForUserInput(); if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE) { MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE; UpdateAllEnvironmentParameters(); OffsetDistOutput = PIDCalcs((double)gl_FrontCm, (float)offsetCm, lastError, lastInput, lastIval, lastDerror, OffsetDistKp, OffsetDistKi, OffsetDistKd); int16_t speed = (int16_t)OffsetDistOutput; //PIDCalcs out can be negative, must handle both signs if (speed >= 0) { speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed; //speed = (speed <= MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : speed; } else //speed < 0 { speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed; } gl_pSerPort->printf("%lu\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%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); } } gl_pSerPort->printf("MTFD: Stopped with front dist = %d\n", GetFrontDistCm());//c/o 06/11/23 - chks now done in HandleAnomalousConditions() StopBothMotors(); return true; } |
This function starts with a measured distance of 18cm and a target of 30. The column header line gets printed OK, but then it immediately stops at a distance of 19cm. Obviously the
1 |
while (abs(gl_LeftCenterCm - offsetCm) > tgt_window)//06/11/23 rem errcode condx |
statement is returning a FALSE value and terminating the block, but I don’t see why
Oops! Using the wrong distance variable – gl_LeftCenterCm instead of gl_FrontCm. Apparently I never ported the correct code from 230918_WallE3_MoveTo_Test.ino to WallE3_Complete_V5.ino.
Stay Tuned,
Frank