Posted 14 October 2024
It’s been almost four months since I did anything with Wall-E3, my autonomous wall-following robot. I’ve been busy with building a new 3D printer (Prusa Mk4) quarrelling with (and losing to) my other 3D printer (Flashforge Creator Pro 2), and some other stuff, but I’m now between projects so I want to spend some more time with Wall-E3. I ran a couple of field tests on my normal home track, and saw that the robot is still having trouble with managing transitions from one wall to another, especially at the end of the kitchen counter (the ‘B’ position shown in the diagram in this post).
Looking through the telemetry log, I was struck by the fact that it appears that the HandleAnomalousConditions() function always called with ANOMALY_CODE == ANOMALY_NONE. Looking through the code, I’m not quite sure where this happens. There is code in UpdateAllEnvironmentParameters(), but it shouldn’t execute unless all the other ‘elseif’s fail. Looking further, I found this code in the HandleAnomalousConditions() function:
|
1 2 3 4 5 |
case ANOMALY_EXCESS_STEER_VAL: gl_pSerPort->printf("In HandleAnomalousConditions(%s) ANOMALY_EXCESS_STEER_VAL case - \n", WallTrackStrArray[trackcase]); gl_LastAnomalyCode = ANOMALY_NONE; //10/01/23 HandleExcessSteervalCase(trackcase);//09/16/23 added trkcase to sig break; |
However, it doesn’t appear that HandleExcessSteervalCase() is being called at all – hmm.
20 October 2024 Update:
So I think I figured some of this out. Here’s the relevant code in loop():
|
1 2 3 4 5 6 7 8 9 10 |
void loop() { CheckForUserInput(); delay(200); //08/17/23 put in to make sure distances are current UpdateAllEnvironmentParameters(); //07/27/23 moved out of IR HOMING region for clarity HandleAnomalousConditions(gl_CurTrackingCase); } |
So the program simply loops through UpdateAllEnvironmentParameters() and HandleAnomalousConditions. The Update function retrieves all sensor information and uses it to update the global variable gl_LastAnomalyCode. HandleAnomalousConditions then uses the value of gl_LastAnomalyCode in a CASE statement to figure out what to do. Here’s the relevant code from HandleAnomalousConditions
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
void HandleAnomalousConditions(WallTrackingCases trackcase) { switch (gl_LastAnomalyCode) { case ANOMALY_EXCESS_STEER_VAL: { //10/14/24 added to allow code block to be hidden gl_LastAnomalyCode = ANOMALY_NONE; //10/01/23 HandleExcessSteervalCase(trackcase);//09/16/23 added trkcase to sig break; } default: break; } } |
The problem I was seeing was, HandleAnomalousConditions() was being called each time, but the ANOMALY_EXCESS_STEER_VAL case was never executed. Somehow, the anomaly code was being changed to ANOMALY_NONE before the call to HandleAnomalousConditions().
I think I finally figured out what was happening. The very first time through loop(), UpdateAllEnvironmentParameters() is called and (normally) sets gl_LastAnomalyCode to ANOMALY_NONE. Then HandleAnomalousConditions() is called and the ANOMALY_NONE case executes. This launches either the TrackRight or TrackLeft functions, which have their own internal loop that doesn’t exit until a non-NONE anomaly is detected. When this happens, the tracking loop exits, but because it started in the ANOMALY_NONE case section of HandleAnomalousConditions(), the next instruction to be executed is the one immediately after HandleAnomalousConditions() exits! The next relevant instruction is the call to UpdateAllEnvironmentParameters() at the top of loop(), which can (and apparently does) modify gl_LastAnomalyCode from ANOMALY_EXCESS_STEER_VAL to ANOMALY_NONE, which means the ANOMALY_EXCESS_STEER_VAL case is never executed.
So, it is critical that nothing changes gl_LastAnomalyCode between HandleAnomalousConditions() calls. As it turns out, the fix was simply to remove the call to HandleAnomalousConditions() at the top of loop(), resulting in the following in loop():
|
1 2 3 4 5 6 |
void loop() { CheckForUserInput(); HandleAnomalousConditions(gl_CurTrackingCase); } |
This works because all functions that contain a local loop (like TrackLeft/RightWall()) call UpdateAllEnvironmentParameters() each time through their loop, and exit when a non-NONE anomaly code is detected. With the removal of the call to UpdateAllEnvironmentParameters() in loop(), the next call to HandleAnomalousConditions() ‘sees’ the correct value in gl_LastAnomalyCode.
I made this change and made another run, with the following telemetry output:
|
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 |
In HandleAnomalousConditions(NEITHER) with last anomaly code = NONE In HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE 0.5: gl_Left/RightCenterCm = 24.6/111.8, Left/RightSteerVal = 0.02/0.34 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.6) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 15394: Top of UpdateAllEnvironmentParameters(LEFT) with anomaly code = NONE 15444: Top of UpdateAllEnvironmentParameters(LEFT) with anomaly code = NONE 0.7 24.3 112.9 0.9 24.2 24.2 0.00 428 54 63806 12694 117 32 NONE LEFT 17788: In UpdateAllEnvironmentParameters(LEFT) IsExcessiveSteerVal(trkdir) section with anomaly code = EXCESS_STEER_VAL 3.0 33.1 67.9 7.1 145.2 30.4 1.00 377 165 8057 12830 0 127 EXCESS_STEER_VAL LEFT Stopping Motors! Anomaly Code is 8 --> EXCESS_STEER_VAL TrackLeftWallOffset() bottom with Anomaly Code EXCESS_STEER_VAL Bottom of HandleAnomalousConditions(NEITHER) with last anomaly code = EXCESS_STEER_VAL Battery Voltage = 7.97 Just before call to HandleAnomalousConditions() with anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL case - Top of HandleExcessSteervalCase(LEFT) 4256: gl_Left/RightCenterCm = 108.1/64.5, Left/RightSteerVal = 0.72/-0.02 HandleExcessSteervalCase(LEFT) open doorway block In RotateToParallelOrientation(RIGHT) RTPO: starting front/rear/steer avgs = 65.86/65.88/-0.00 GetWallOrientDeg(-0.00) returned starting orientation of -0.11 deg bypasssing RotateToParallelOrientation for off-angle = -0.11 TrackRightWallOffset(350.0, 0.0, 20.0, 30) called TrackRightWallOffset: Start tracking offset of 30cm at 6.3 sec Calling CaptureWallOffset(TRACKING_RIGHT, 64.9) SpinTurn: Init hdg = 0.56 deg, Turn = 90.00 deg, tgt = 90.56 deg, timeout = 4.00 sec After SpinTurn, Hdg/Dist = 89.0/65 MTFD: at start, tgt = 30cm, curr_dist = 66, PID = 1.50, 0.10, 0.20 Msec Fdist TgtD err Ival Out Speed FVar RVar 23236: Top of UpdateAllEnvironmentParameters(NONE) with anomaly code = NONE In DoOneMoveToFrontDistCm() at top, displaying 10 front dist entries Msec FrontD TgtD Err Ival Output speed FVar RVar ACODE MTDFD: Exited with front dist = 31, anomaly code = WALL_OFFSET_DIST_AHEAD delaying 1sec to allow distances to settle Front Rear Left Right 21 176.30 283.90 225.90 Second time through with error = 9 In DoOneMoveToFrontDistCm() at top, displaying 10 front dist entries |
In the above telemetry, the significant points are:
- The first pass through HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE decides which wall to track and selects TrackLeftWallOffset()
- Tracking starts at 0.7Sec and ends at 3.0Sec when an EXCESS_STEER_VAL anomaly is detected.
- In HandleAnomalousConditions(LEFT) with last anomaly code = EXCESS_STEER_VAL shows that the gl_LastAnomalyCode has not been changed
- In HandleAnomalousConditions(LEFT) ANOMALY_EXCESS_STEER_VAL case shows that the proper case section is executing.
- HandleExcessSteervalCase(LEFT) open doorway block shows that the appropriate handler function is launched.
- Ultimately this results in the right wall being captured and tracked (the run was terminated just before the robot actually started tracking the righthand wall).