Monthly Archives: August 2023

WallE3_Complete_V5 Code Cleanup

Leave a reply

Posted 25 August 2023

After (I hope) getting WallE3’s wall-switching feature figured out, I am trying to clean up the code in WallE3_Complete_V5:

  • Enums: Removed NavCases, TrackingState enums. Removed ‘OPEN_CORNER’, ‘OPEN_DOORWAY’ and ‘TRACKING_WRONG_WALL’ anomaly codes from AnomalyCode enum list, and corresponding strings from AnomalyStrArray. Removed TRACKING_LEFT/RIGHT_CAPTURE entries from WallTrackingCases enum list, and corresponding strings from WallTrackStrArray. Now there are only three enums in enums.h.
  • Cleaned out unused/commented-out PID values from WALL_FOLLOW_SUPPORT #pragma region
  • Deleted previously commented-out global boolean variables (see this post for details)
  • Removed dead code from loop() #pragma region WALL_TRACKING section. Now this section contains only one line – ‘HandleAnomalousConditions(gl_CurTrackingCase);’
  • Removed dead code from UpdateAllEnvironmentParameters()
  • Removed IsOpenCorner() (was already commented out)
  • Removed IsTrackingWrongWall() (was already commented out)
  • Removed CheckForAnomalies() (was already commented out)
  • Added braces to ANOMALY_NONE: case in HandleAnomalousConditions() to eliminate any possible scoping issues.
  • Removed dead code from ExecuteStuckRecoveryManeuver()
  • Removed dead code from ExecuteRearObstacleRecoveryManeuver()

When done, The code cleanup resulted in about 700 fewer lines of code (~6400 vs ~7100)

Troubleshooting Wall Switch Maneuver

Leave a reply

Posted 18 August 2023

I’m running some ‘open doorway’ tests in my office test setup, and I keep getting an uncontrolled spin maneuver at the end of the run. The following short video illustrates the problem:


And here is the telemetry for the above run:

The very first time through the program, gl_LastAnomalyCode is ANOMALY_NONE, so the CASE statement gets skipped entirely (this is why there are two “Just after UpdateAllEnvironmentParameters() at top of loop()…” printouts here, but only one thereafter).

At 2.7Sec an anomaly (ANOMALY_EXCESS_STEER_VAL) is detected when the robot hits the gap in the right-hand wall. This causes the program to start over at the top of loop(), and this time gl_LastAnomalyCode = ANOMALY_EXCESS_STEER_VAL, which causes HandleExcessSteervalCase() to be called. HandleExcessSteervalCase() in turn calls TrackLeftWallOffset(), which in turn calls CaptureWallOffset(TRACKING_LEFT,78.4).

The capture operation moves the robot to the ‘other’ wall and turns the robot parallel, and then continues TrackLeftWallOffset().

From 19.0 to 21.2Sec the robot tracks the left wall, where it again detects a ANOMALY_EXCESS_STEERVAL anomaly. Unfortunately, due to the proximity of a bookshelf on the left, the reported left/right distances at this point are 35.9/63.7cm, so the program calls TrackLeftWallOffset() instead of TrackRightWallOffset(). Because the starting distance (3.59Cm) is less than 1.5*[desired offset of 30cm], the capture phase is skipped entirely and TrackLeftWallOffset() is continued. The column header line is printed, but the program immediately detects an ANOMALY_EXCESS_STEERVAL anomaly. From the video it looks like the robot should have tracked the left wall (the book case) for a second or so.

The bottom line (I think) is that I had a flawed test wall configuration; I wanted the robot to transition back from the left wall to the right one, but the end of the left wall was too close to the actual left wall of my office – bummer.

23 August 2023 Update:

I think I may have finally gotten the multiple-wall-switch scenario working properly! Here’s a short video and the telemetry for the run:

The robot starts out tracking the right wall. At about 3sec, it hits the first wall gap and generates an EXCESSIVE_STEER_VAL anomaly. This caused the program to exit wall tracking and restart the loop() function from the top.

It took another 2.7sec to figure out it needed to transition to the left wall, calling “TrackLeftWallOffset: Start tracking offset of 30cm” at 5.7sec. This involved an ‘offset capture step, so the actual left wall tracking phase didn’t begin until about 13sec.

At 15.8sec the robot generated another EXCESSIVE_STEER_VAL anomaly, again causing the program to exit wall tracking and restart the loop() function from the top. This time the anomaly was caused by running out of wall on the left side, so the robot has to transition back to the right wall – that was what was supposed to happen. What actually happened is the robot still thought there was a left wall available at 36.4cm and a right wall at 52.4cm, so it started tracking the (nonexistent) left wall again. Of course this cause an immediate EXCESSIVE_STEER_VAL anomaly, and this time the left/right distance was 127.9/50.6, so this time the robot correctly transitioned back to the right wall, starting at 19.2sec. The transition involved an ‘offset capture’ step, so the actual right-wall tracking operation started at 29.1sec.

All in all, I thought this was a very successful run, with the minor nit about momentarily trying to track a non-existent wall. I think this may be an instance where distance reporting is lagging slightly behind reality.

24 August 2023 Update:

I added a second 200mSec delay and a second call to ‘UpdateAllEnvironmentParameters()’ to troubleshoot the above ‘lagging distance problem’, but it didn’t solve the problem – still get the same problem with a ‘phantom left wall’ measurement. After looking at the code a bit, I now see there is a definite coding problem – oops!

HandleAnomalousCondition vs Switch(gl_LastAnomalyCode)

Leave a reply

Posted 14 August 2023

In previous versions of the WallE3 operating system I used three functions to deal with anomalous conditions. At each update interval (currently set to 50mSec) I called a function called UpdateAllEnvironmentalParameters() which updated all sensor measurements; then a second function called CheckForAnomalousConditions() to update anomaly condition flags (like ‘gl_DeadBattery’, ‘gl_bStuckAhead’ and the like), and a third function, called HandleAnomalousConditions() to actually respond properly to any anomalous condition identified in CheckForAnomalousConditions().

However, I wasn’t really happy with the program flow with this arrangement, so in the current version (WallE3_Complete_V4) I eliminated CheckForAnomalousConditions() entirely and pulled the anomaly flag update code into UpdateAllEnvironmentalParameters(). I also replaced HandleAnomalousConditions() with a ‘switch(gl_LastAnomalyCode)’ block at the top of the loop() function, just before the code that decides which wall (left or right) is to be tracked. So now the program flow looks like this:

So, very simple and very direct. The ‘case’ block looks like this (only the ‘ExcessSteerVal’ case has been populated at the moment):

I think this is going to be a robust flow; Any anomaly causes the current tracking operation to break and return control to the top of loop(). Then the case statement handles the anomaly as required, and drops the program right back into left/right wall tracking determination. If no anomalies are encountered (unlikely, but could happen) then the tracking block runs forever.

15 August 2023 Update:

I’m working on populating the ‘ANOMALY_STUCK_AHEAD’ case in the ‘switch (gl_LastAnomalyCode)’ block. The function ExecuteStuckRecoveryManeuver(WallTrackingCases trkdir) already exists, but it requires a ‘WallTrackingCases trkdir’ parameter to specify which wall is currently being tracked. I have a global ‘TrackingCases’ variable called ‘gl_CurTrackingCase’, but it isn’t clear to me how it is initiated and updated in the code.

Searching for ‘gl_CurTrackingCase’ produces the following hits:

  • it is initialized to ‘WallTrackingCases::TRACKING_NEITHER’ in the pre-setup initialization block
  • It is set to ‘TRACKING_LEFT’ at the top of TrackLeftWallOffset()
  • It is set to ‘TRACKING_RIGHT’ at the top of TrackRightWallOffset()
  • In ‘HandleExcessSteervalCase()’ it is updated to either TRACKING_LEFT or TRACKING_RIGHT, depending on current left/right distances, and then used as the input to several ‘RotateToParallelOrientation()’ and ‘SpinTurn()’ calls.

Looking at the above list, it appears that the value stored in gl_CurTrackingCase by the TrackLeftWallOffset() & TrackRightWallOffset() functions is never used; in HandleExcessSteervalCase() (the only function that references gl_CurTrackingCase), the value of the variable is updated locally by checking current left/right wall distances. IOW, the initialization lines in TrackLeftWallOffset() & TrackRightWallOffset() could be removed and the references to gl_CurTrackingCase in HandleExcessSteervalCase() could be converted to local variables and nothing would change.

So it appears that there are two ways to skin this cat.

  • Keep gl_CurTrackingCase as a global variable that is updated in TrackLeftWallOffset() & TrackRightWallOffset() to reflect the current tracking case, and then reference it in HandleExcessSteervalCase() without regenerating the value from left/right wall distances. Keep the current definition of ExecuteStuckRecoveryManeuver(WallTrackingCases trkdir) and call it from the ‘ANOMALY_STUCK_AHEAD’ case block, with ‘trkdir’ replaced by gl_CurTrackingCase.
  • Remove the ‘gl_CurTrackingCase’ global variable entirely, and use current left/right distances to determine the tracking case anywhere it is needed.

Of these two options, I prefer the first one. At the point where either TrackLeftWallOffset() & TrackRightWallOffset() are called, the tracking case is known current left/right distance comparisons, and inside either tracking function, the tracking case doesn’t change. When the active tracking function exits to the top of loop() due to an anomaly (the only way it can exit), the extant tracking case is still what it was at function exit. The only way it should change is from another left/right distance comparison after the current anomaly has been resolved.

16 August 2023 Update:

Oops! I ‘simplified’ HandleExcessSteervalCase() and now it doesn’t work anymore – ugh! The problem is I didn’t have a good understanding of how this function decides which wall to track. Its usually (but not necessarily) the other wall from the one identified in gl_CurTrackingCase, so I actually have to check left/right distances as was done in the ‘unsimplified’ version. So most of the time, HandleExcessSteervalCase() will change gl_CurTrackingCase to the ‘other’ wall

19 August 2023 Update:

Well, things are a bit more complicated than I thought. While chasing other bugs, I realized that there are other places in the code that use anomaly detection – oops. Now I have a CASE block at the top of loop() that I thought replaced the code in HandleAnomalousConditions(), but now I see that it only replaced one usage – there are several more places in the program that still use the function

  • 2 places in ExecuteStuckRecoveryManeuver()
  • MoveToDesiredRightDistCm()
  • MoveToDesiredLeftDistCm() – it should be in there but isn’t – strange!
  • MoveToDesiredFrontDistCm() – it should be in there but isn’t – strange!
  • MoveToDesiredRearDistCm() – it should be in there but isn’t – strange!

So this is a real problem – I have a CASE block doing anomaly handling at the top of loop(), and HandleAnomalousConditions() doing the same thing (but different code!) in several other places in the program – yikes!

Here is the code for HandleAnomalousConditions:

So HandleAnomalousConditions() is just a bunch of if and ‘else if’ blocks looking at a bunch of global boolean variables denoting different anomaly conditions. The boolean variables are updated in UpdateAllEnvironmentParameters(), and the associated anomaly code is loaded into gl_LastAnomalyCode.

So, the current program flow associated with anomalies goes something like this:

  • UpdateAllEnvironmentParameters() is called each time through the timing loop for any function that has a timing loop. It updates all the anomaly-related global boolean variables.
  • HandleAnomalousConditions() is also called each time through, and it both updates gl_LastAnomalyCode with the current anomaly condition, AND actually takes action to address the anomaly condition (either directly or by calling a specific handler function)
  • The new CASE block at the top of loop() switches on the value of gl_LastAnomalyCode (as updated by HandleAnomalousConditions()) and ALSO attempts to address the current anomaly condition before dropping back into either TrackLeftWallOffset() or TrackRightWallOffset().

So it appears that the CASE block and HandleAnomalousConditions() are doing the same thing, but at different program scope levels; The CASE block only runs at the top of loop(), but HandleAnomalousConditions() is ‘local’ to most functions that have their own ‘local’ operating loops. Moreover there are two duplicative ways to describe anomalies – the various global boolean variables like ‘gl_bStuckAhead’ and the global enumerated values for gl_LastAnomalyCode like ‘ANOMALY_STUCK_AHEAD’.

The idea behind the enumerated anomaly codes was to consolidate anomaly detection in ‘while’ loops to just something like ‘while (gl_LastAnomalyCode == ANOMALY_NONE)’ rather than having to list all applicable error conditions with something like ‘while(!gl_bStuckAhead && !gl_bStuckBehind && !gl_ObstacleAhead …). If an anomaly was detected, then the idea was that the function would exit in a way that caused the main loop() function to run again from the top, and the CASE block would actually respond to the anomaly. In this scheme, error handling is removed from the context in which the error occurred – probably OK for TrackLeft/RightWallOffset(), but not so much for MoveToDesiredFront/Back/Left/RightDistance() as the potential anomalies are few and the response to those errors are heavily context dependent (I think).

So now I’m beginning to think that this entire ANOMALY_XXX thing (with accompanying enum.h) is a bust, and unneeded. Part of the reason for doing it was to (as noted above) avoid having long strings of “!gl_bStuckAhead && !gl_bStuckBehind && !gl_ObstacleAhead …” in the ‘while’ statements. It just occurred to me that maybe I could consolidate these into a function call, like ‘IsAnomaly()’ that would return TRUE if any anomaly was found; so now the ‘while’ statement would look like:

And ‘IsAnomaly() would just be a big OR string.

So I can replace the big CASE statement at the top of loop() with just

if (gl_bExcessiveSteerVal) {....}

OK, so I created another ‘clean’ version – WallE3_Complete_V5 to see whether or not I can remove the ANOMALY_XXX stuff without completely screwing up the code. First I made sure that _V5 would compile clean – check. Then I commented out the ANOMALY_XXX lines from enum.h. This blew a bunch (as in 28) of errors, so I started going through them from top to bottom:

Not going to work; I can (and did) create the IsAnomaly() function, but using it instead of ‘&& gl_LastAnomalyCode == ANOMALY_NONE eliminates the ability to print out the name of the anomaly that caused the loop to break – and I want to keep this feature.

I guess I could simply replace the code in ‘HandleAnomalousConditions() with the CASE block that switches on gl_LastAnomalyCode, and that would at least get me to the point of having only ONE function that deals with anomaly handling.

So, first order of business is to try and eliminate the global gl_bXXX anomaly variables: In UpdateAllEnvironmentParameters() we have:

So a typical line like


would be replaced with:

This would allow human-readable printout of the exact anomaly type, and the use of a CASE block with

  • Replaced all gl_bxxx lines in UpdateAllEnvironmentParameters() with ‘if(xx) statements & recompiled – OK
  • Replaced the code in ‘HandleAnomalousConditions()’ with a CASE block vs ‘if’ and ‘elseif’ statements – Recompiled OK.
  • Replaced the ‘switch (gl_LastAnomalyCode)’ statement at the top of loop() with a call to HandleAnomalousConditions(gl_CurTrackingCase). Recompiled OK
  • Did a search for each gl_bXXX to make sure it was no longer used:
    • gl_bIRBeamAvail: moved the ‘if(gl_IRBeamAvail)’ code inside #pragma region IR_HOMING into the ANOMALY_IR_BEAM_AVAILABLE case inside HandleAnomalousConditions. The call to UpdateIRHomingValues() here isn’t needed – it is called by IsIRBeamAvail() in UpdateAllEnvironmentParameters(). Also removed it from ‘while (gl_LastAnomalyCode == ANOMALY_NONE && !gl_bIRBeamAvail)’ statement in TrackLeft/RightWallOffset(). Now compiles OK w/o gl_bIRBeamAvail defined.
    • gl_bChgConnect: Can’t remove because it is state memory for charge connnect/disconnect state.
    • gl_bObstacleAhead: Compiles OK w/o gl_bObstacleAhead
    • gl_bWallOffsetDistAhead: Compiles OK w/o gl_bWallOffsetDistAhead
    • gl_bObstacleBehind: In ExecuteStuckRecoveryManeuver() replaced ‘gl_bObstacleBehind’ with equivalent ANOMALY_CODE treatments
    • gl_bStuckAhead/Behind: Basically the same as for gl_bObstacleBehind
    • gl_DeadBattery: No changes required; the only place it was used had already been replaced by a call to IsDeadBattery() and assignment of ANOMALY_DEAD_BATTERY to gl_LastAnomalyCode.
    • gl_bTrackingWrongWall: No changes required; It never found a home in the current program – and there is no existing comparable ANOMALY_CODE enumeration.
    • gl_bOpenCorner/gl_bOpenDoorway: no No changes required; these anomalies are both handled as sub-cases of ANOMALY_EXCESSIVE_STEERVAL
    • gl_bIsSpinning: No changes required; It never found a home in the current program – and there is no existing comparable ANOMALY_CODE enumeration. This may come back at some time – but if it does, it will be via an added ANOMALY_CODE enumeration – not a global bool.
    • gl_bExcessiveSteerVal: No changes required; Replaced by a call to IsExcessiveSteerVal() and assignment of ANOMALY_EXCESS_STEER_VAL to gl_LastAnomalyCode.

OK – now I have eliminated all global booleans associated with anomalies – everything now is handled in the switch(gl_LastAnomalyCode) CASE block. At this point I’m going to commit to my local Git repository – with all the old code still in the codebase but commented out until I can run some basic tests to see how badly I have screwed everything up.

22 August 2023 Update:

This morning I uploaded _V5 to the robot and set it loose on my ‘two wall changes’ test configuration, and I’m happy to say that it performed identically to what it was doing prior to the changes described above. It even failed in almost exactly the same way at the end with the ”aggressive spin move” behavior (which I’m almost sure is due to calling SpinTurn() with a very large rotation value).

24 August 2023 Update:

I think I now have the ‘right-left-right’ wall switching algorithm working fairly well now, and the actual code is starting to look a lot like the flow diagram posted above. The latest change was to move left/right wall selection code out of loop() and into ‘HandleAnomalousConditions()’, bringing the code more into line with the flow diagram. Now the robot goes through both transitions (right-to-left and left-to-right) with no problem and no duplicative steps – yay!

Stay tuned,

Frank