Author Archives: paynterf

The Guest Bedroom Problem

Leave a reply

Posted 01 October 2023

After solving the problems described at the bottom of my “Responding to Steerval out of Range” post, I was able to get WallE3 to successfully enter our small guest bedroom, as shown in the following short video:

Unfortunately the robot wasn’t really tracking the left ‘wall’ (door surface) – it was just going straight until it ran into the trashcan, and then couldn’t figure out what to do next. As I worked with the code I got WallE3 to track the door OK, but then it still had problems with the trashcan and cupboard. Rather than starting in the hallway for each test, I decided to emulate the configuration in my office for more rapid iterations. Here’s the actual guest bedroom configuration:

And here is the simulated guest room configuration in my office:

Simulation of guest room configuration

And here is a short video showing a mostly successful run in the above simulated guest room configuration:

The telemetry from the run is shown below:

The salient events from the telemetry:

  • 0.6 – 2.1 sec: tracks left wall
  • 2.1 sec: detects WALL_OFFSET_DIST_AHEAD anomaly.
  • 2.1 – 7.6 sec recovers from WALL_OFFSET_DIST_AHEAD and starts tracking left wall again
  • 7.7 – 9.2 sec tracks left wall (simulated doorway into guest br)
  • 9.2 sec detects WALL_OFFSET_DIST_AHEAD anomaly (end of simulated door?)
  • 9.3 – 10.7 sec: recovers from WALL_OFFSET_DIST_AHEAD and starts tracking left wall again
  • 10.7 – 12.1 sec: tracks left wall
  • 12.1 sec: detects WALL_OFFSET_DIST_AHEAD anomaly. I think this is caused by the box (the simulated near end of the cupboard). This time the anomaly recovery calls ChooseBetterTrackingSide() with an average heading value of 93.1º. The result of this algorithm is 9 votes for left side, 0 for right. The robot starts tracking the left wall with an initial offset of 17.9cm, which is too close, and this causes ‘CaptureWallOffset()’ to be called. CaptureWallOffset() makes a 90º CCW turn to face the left wall, and then calls ‘MoveToDesiredFrontDistCm()’ to move (backup) to a front distance of 30cm. Once this is accomplished, a 90º CW turn is made to re-parallel the left wall, and then ‘RotateToParallelOrientation(LEFT)’ is called to complete the orientation operation.
  • 29.9 – 31.9 sec: tracks left wall (the simulated cupboard face).
  • 31.9 sec: an EXCESS_STEER_VAL anomaly is detected after the robot runs off the end of the simulated cupboard face

02 October 2023 Update:

Having solved the problem of getting from the hallway into the guest bedroom, there remained the problem of negotiating a ‘cul-de-sac’ at the far end of the cupboard, which I simulated in my office as shown in the following video:

And here is the telemetry from the run:

The robot tracks the left wall properly for the first 2.5sec, but then gets trapped in the ‘cul-de-sac’, and eventually resorts to calling a ‘RunToDaylight()’ function to get itself unstuck. RunToDaylight() calls ‘RotateToMaxDistance()’ which performs a full 360º rotation, memorizing the front distance and heading at each step. After the full rotation, the robot turns to the heading associated with the peak front distance and heads that way. As can be seen from the video, this works very well, but over the course of several runs the robot hit the corner of the wall a couple of times because the ‘peak’ heading was recorded just as the front distance sensor (but not necessarily the robot itself) cleared that corner. Here’s an excel plot that shows the issue.

Looking at the plot, it is clear that any heading from about 83 to 150º would suffice, so maybe picking the peak value isn’t so smart. Maybe picking the median value above some distance threshold (like maybe 1.5-2.0 meters) would be a better bet.

04 October 2023 Update:

As I was drifting off to sleep last night, I was mentally reviewing the above ‘run to daylight’ experiment and it struck me that WallE3 had ignored a perfectly good trackable wall segment (at about 31sec, step 22, 94.4º, 215cm in the above 360º search). It occurred to me that I should be searching for the point(s) where WallE3 is parallel or mostly parallel to a wall, with a meter or more of front distance available.

I think I might be able to use a “Figure of Merit” (FoM) defined as the front distance divided by the left or right steering value to detect the optimum travel direction. Steering values vary from 0 to +/-1 so it would need to be abs(FrontD/SteerVal), and it would be maximum at the point where steerval is minimum and front distance is maximum.

One fly in the ointment is the case where steerval is == 0; that would cause FrontD/steerval to be undefined – oops! So I think I need to modify the function that retrieves steervals from the Teensy to limit steerval to something like .001, so the FoM wouldn’t go above a few thousand (10/07/24 note: This was done in ‘GetRequestedVL53l0xValues()’)

Also, I need to figure out how to sort through the 360º search results. I would like to wind up with an array of steervals sorted in decreasing magnitude order, with a companion array of headings so Hdg[i] <–> Steerval[i]. Not sure how to do this yet, but it sounds promising!

07 October 2023 Update:

After some web searching and experimentation, I now have a program that can sort a ‘master’ array and one or more ‘slave’ arrays such that the slave array is rearranged into the same order as the master. See this post for all the gory details.

Now I think I have the tools to address the ‘guest bedroom problem’, as follows:

  • Make the same 360º rotation as before, saving L/R steervals, headings, and front distances into separate 36-element arrays.
  • Sort the above arrays, using front distances as the master, and the others as slaves.
  • Find the element in the front distance array that is greater than a set threshold (say 150cm), has an associated steerval <= (MAX_STEERVAL / 2) and produces the highest ‘FoM’ as described above. Turn to the heading associated with this element, and start tracking that side.
  • If no front distance element / steerval combination satisfies the above criteria, then turn to the heading associated with the middle element of the first/only set of front distance values above the above distance threshold and move forward until another anomaly is detected.

11 October 2023 Update:

I think I’m going to need to save/sort the L/R distances in addition the values listed above, for a total of six parameters; L/R distances, L/R steervals, Hdg, and Front distance. The FrontD array would be the ‘master’ array, and the other five would be ‘slave’ arrays. I could modify my current QuickSort() routine to do all five slave arrays at once (by adding four more ‘swap’ lines in two places in ‘partition’) but this would also require modifying the signatures of QuickSort(), partition() – yuk! I could also simply call ‘QuickSort()’ five times (once for each ‘slave’ array) with the unsorted FrontD array as the master each time. I like this latter approach as it preserves more of the generality of the ‘QuickSort()’ routine.

To do this, I’ll have to define all six arrays at global scope with the following memory requirements:

  • uint16_t FrontD[36]: 2 bytes/value, 36 values –> 72 bytes
  • float Hdg[36]: 4 bytes/value, 36 values –> 144 bytes
  • float LeftSteer[36]: 4 bytes/value, 36 values –> 144 bytes
  • float RightSteer[36]: 4 bytes/value, 36 values –> 144 bytes
  • float LeftDist[36]: 4 bytes/value, 36 values –> 144 bytes
  • float RightDist[36]: 4 bytes/value, 36 values –> 144 bytes

for a total of 4*144 + 72 = 648 bytes. The current memory requirements for WallE3_QuickSort_V3 on a Teensy 3.5 is:

So another 648 bytes is not going to be a problem. After defining and initializing the additional four arrays and recompiling, I got:

So the increase from 115248 to 115312 in program size and from 7728 to 8304 bytes wasn’t enough to even move the percentages – woohoo!

16 October 2023 Update:

After getting the ‘one master and five slave array’ quicksort algorithm working, I did some more testing in my office. The test configurations that featured side walls within tracking distance (100cm) worked very well; however, when I simulated the configuration where no trackable side walls could be found, the robot consistently failed to orient to the most open direction – instead it wound up pointing to one edge or the other of the ‘open direction’ wedge. The algorithm I was using was to use the heading associated with the middle entry of the first N (reverse sorted) front distances.

Then I tried using the middle entry of the first N (reverse sorted) heading values, where the group of heading values to be sorted were the ones associated with the above first N reverse-sorted front distances. However, this didn’t work either, because when I designed the experiment, the maximum distances naturally occurred around 180º from the starting orientation, and this meant that the first N headings included both positive and negative values. When reverse-sorted, the positive values were at the top as expected, but the negative values got sorted to the bottom – oops!

The solution to the problem was to create a temporary heading array, fill it with the heading values associated with the first N front distances as before, but with the heading values modified to fit into the 0-359º range (by adding 360 to negative values). The result of this trick was to ‘homogenize’ the headings so they were all sequential, and then selecting the median value did indeed point the robot in the correct direction, as shown in the following short video and telemetry output.

In the above telemetry output, the first two values in the Hdg array associated with the reverse-sorted FrontD array are negative, so if the Hdg array is reverse sorted to find the best RunToDaylight heading, the two headings associated with the longest open run aren’t considered unless they are first modified such that all headings lie in the 0-360 range. The ‘tempHdg’ array accomplishes this.

After the above successful test of the new ‘RunToDaylight’ capability, it is time to integrate this into the rest of the program. At the moment the code is contained in ‘RunToDaylightV2’ and it is only called from ‘#pragma region QUICKSORT_TEST in setup(). In the mainline code, ‘RunToDaylight’ is called from ChooseBetterTrackingSide() when the ‘Better Side’ can’t be determined.

ChooseBetterTrackingSide() is called from HandleExcessSteervalCase() when a trackable wall is found on both sides, and a determination has to be made on how to proceed. So, it appears that all I have to do is comment out (or delete) the QUICKSORT_TEST #pragma region and RunToDaylightV2 will be called as necessary.

After making the necessary changes, I made a long ‘field test’, starting in our entrance hallway and going through the kitchen and down the bedroom hallway to the end, and then once again into the guest bedroom. This all worked perfectly, and I was pleased to see that ‘RunToDaylightV2()’ was called at the end. See the accompanying video and the telemetry for all the details:

17 October 2023 Update:

I tried another Guest Bedroom ‘field test’, with different results this time, as shown in the accompanying video and telemetry:

Stay tuned,

Frank

WallE3_Complete_V5 Print Timing Issue

Leave a reply

Posted 24 September 2023,

This post addresses the problem I have started to see regarding print buffer overflow due to the decreased time (now 50mSec) between environment updates. When running longer field tests, I have started to see many lines where telemetry printout has become garbled, with the next line of printout starting before the previous line has finished, as shown in the accompanying printout

In the above printout, you will notice that the horizontal scrollbar is much smaller than normal, indicating the line lengths are much longer than normal, and the portion that is visible is really really ugly!

So, what to do? I ran a search through my almost 7000 lines of code for ‘gl_pSerPort->printf(‘ to see how big of a problem I’m looking at, and the search came back with some 400+ hits – ouch! One thought would be to put an ‘if’ statement around each printf call and use an ‘EllapsedMillisec’ variable to limit calls to, say, every 100 or 200 msec, but doing that for 400+ calls would get pretty tedious. Another thought is to abstract all these print statements into a function, and have the time spacing internal to the function. So the function call would look something like:

and the function internals would look something like:

The problem with the function internals is the argument list would have to match the contents of ‘formatstr’, which means I would have to have an overload for every unique format, and changing anything would be a hassle, as I would have to find and edit the correct overload – yuck!

OK, more thought needed on this – maybe the ‘if’ statement is the way to go (maybe a macro?)

Another thought that just came up; if I filter all print statements by an elapsed time gate, that might actually filter out important one-time error information – ouch! This potential problem suggests adding a priority value to the ‘LogPrint()’ function, so the function might look like:

And good programming technique would demand that the PRIORITY_CODE have a default value, which (for Arduino programming) would require that every overload have a function declaration at the top of the program – yuck! yuck!

Maybe it would be easier to just put the

around each and every ‘low priority’ print statement (all 400+ of them)? yuck! yuck! yuck!

After thinking about this some more, it occurs to me that this problem may be a lot smaller than I think. The primary culprit is the long telemetry string printed out each WALL_TRACK_UPDATE_INTERVAL_MSEC (currently set to 50mSec). It may well be that I only need to guard this one print statement and 99% of the time that will do the trick. As I find other printout statements that are causing problems, I can treat them as well.

25 September 2023 Update:

I did a search for ‘gl_pSerPort->printf(‘ but bypassed everything in setup() and loop(). Starting in TrackLeftWallOffset() in the ‘while (gl_LastAnomalyCode == ANOMALY_NONE )’ block I put an ‘if(mSecSinceLastTelemetryUpdate > PRINT_INTERVAL_MSEC)’ guard around the call to ‘OutputTelemetryLine(TRACKING_LEFT)’, as shown below:

26 September 2023 Update:

After guarding the ‘OutputTelemetry()’ call in both TrackLeftWallOffset() and TrackRightWallOffset(), I ran another field test, and saw that telemetry output was clean, and now coming in 0.1sec intervals, as intended – yay!

Stay tuned,

Frank

Garmin LIDAR-Lite V4/LED Distance Compensation

Leave a reply

Posted 20 September 2023

Last February I changed out the Pulsed Light ‘Blue Label’ LIDAR system on WallE3 for the newer and better Garmin LIDAR-Lite V4 LED time-of-flight distance sensor (see this post and this post), and it has been working very well for me since then. However, I recently had occasion to re-visit my ‘MoveToDesiredForwardDistance()’ function, and found that WallE3 wasn’t doing a very good job of stopping where it should; it was often stopping 8-10cm farther away from the wall than desired.

Going back through the above two posts from last February, I found that this was something I had noticed at the time, but hadn’t worried about as precise front distances are generally not needed. However, precise front distances are required for the ‘MoveToDesiredForwardDistance()’ operation, so it is time to think about distance compensation, at least for distances less than 1m.

I started by redoing the distance measurement experiment shown in my initial study, as shown in the photo below:

Distance Compensation Measurement Setup

This produced the following plot:

Then I took the average from each of the above ‘stairsteps’ and plotted that against the actual distances, arriving at the following plot:

Avg LIDAR Distance, Meas Distance, and Calculated Compensated Distance

In the above chart, I used Excel’s ‘trendline’ tool to display the ‘best fit’ linear equation for both the plot of average values and the plot of actual distances. Of course the equation for the actual distances is exact, but the average values show a consistent too-high measurement.

Then I created a compensation function to make the LIDAR values look like measured values, and came up with y_comp = =0.987*(y_meas – 4.77), and plotted this line (shown in gray above). This compensation works very well for values down to about 40cm, but not so much below that. A separate compensation equation may be required for this section.

21 September 2023 Update:

I modified WallE3’s ‘GetFrontDistCm()’ function to include the above compensation expression and re-ran the test from yesterday, with the following result:

Compensated LIDAR Distance Measurements

As can be seen in the above plot, the compensated distance measurements track the actual distances quite nicely. This should result in the ‘MoveToDesiredFrontDistCm()’ function behaving much better than it did before front distance compensation.

Stay Tuned!

Frank

Responding to ‘Steerval out of Range’ Anomalies

Leave a reply

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:

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

Then I added code to UpdateAllEnvironmentalParameters() to update the average heading array, as shown below:

Then I created a ‘ChooseBetterTrackingSide()’ function to actually determine which side to ‘nominate’ for further wall tracking, as shown below:

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:

Robot very rapidly rotated from ~0deg to ~90deg

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:

Choose Better Side algorithm test

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:

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:

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.

Successful ‘Open Corner’ Run

Here is the current ‘state of code’ for the ‘HandleExcessSteervalCase’ function:

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’:

Successful ‘open doorway’ and ‘open corner’ 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:

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:

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:

So BackupAndTurn90Deg(false) is called which should result in a backup to 30cm, then a 90º CW turn. Looking at BackupAndTurn90Deg(false), we see

So BackupAndTurn90Deg() calls MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM) which should back up to 30cm.

In MoveToDesiredFrontDistCm() we have:

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

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

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

Using Out-of-Range Steerval for Anomaly Detection

Leave a reply

Posted 29 July 2023

WallE3 has to be able to handle anomalous conditions as it wanders around our house. An anomalous condition might be running into an obstacle and getting stuck, or sensing an upcoming wall (in front or in back). It generally does a pretty good job with these simple situations, but I have been struggling lately with what I refer to as ‘the open door’ problem. The open door problem is the challenge of bypassing an open doorway on the tracked side when there is a trackable wall on the non-tracked side. The idea is to simplify WallE3’s life by not having it dive into every side door it finds and then have to find its way back out again. Of course, I could just close the doors, but what’s the fun in that?

My current criteria for detecting the ‘open door’ condition is to look for situations where the tracking-side distance increases rapidly from the nominal tracking offset distance to a distance larger than some set ‘max tracking distance’ threshold, with the additional criteria that the non-tracking side distance is less than that same threshold. When this criteria is met, WallE3 will switch to tracking the ‘other’ side, and life is good.

However, it turns out that in real life this criteria doesn’t work very well, as many times WallE3’s tracking feedback loop sees the start of the open doorway just like any other wall angle change, and happily dives right into the room as shown at the very end of the following short video – oops!

In the above video, the robot easily navigates a 45º break at about 7-8 sec. At about 12 sec, an open doorway appears on the left (tracked) side, and the back of a kitchen counter appears on the right (non-tracked) side. What should happen is the robot will ‘see’ the left-side distance increase past the ‘max track’ threshold, while the right-side distance decreases below it, causing the robot to shift from left-side to right-side tracking. What actually happens is the left-side distance doesn’t increase fast enough, and the robot happily navigates around the corner and into the room.

So, what to do? I started thinking that the steering value (5th column from left) might be a reliable indicator that an anomaly has occurred that may (or may not) need attention. In the telemetry file below, the steering value goes out of range (-1 to +1) in two places – at the 45º break (7.9 – 8.1sec), and again at the ‘open doorway’ event at the end (13.3 sec). This (out-of-range steering value) condition is easy to detect, so maybe I could have the robot stop any time this happens, and then decide what to do based on relevant environment values. In the case of the 45º break, it would be apparent that the robot should continue to track the left-side wall, but in the case of the open doorway, the robot could be switched to right-side tracking.

The function that checks for anomalous conditions is UpdateAllEnvironmentParameters(WallTrackingCases trkdir), shown below:

Hmm, I see that I tried this trick before (last May) but commented it out, as shown below:

Looking back in time, I see a note where MAX_STEERING_VAL was added in September of 2022 for use by ‘RunToDaylight()’, which calls another function called ‘RotateToMaxDistance(AnomalyCode errcode)’. However, neither of these are active in the current code.

OK, back to reality. I plan to change the MAX_STEERING_VAL constant from 0.9 to 0.99, so steering values of 1 will definitely be larger, and most other values will not be.

Then I plan to uncomment ‘IsExcessiveSteerVal(trkdir)’, and the call to it in ‘UpdateAllEnvironmentParameters(trkdir)’. This should cause TrackLeft/Right to exit when the steering value goes to 1 as it does at the 45 break and the open doorway. Then, of course, the question is – what to do? I plan to have the robot stop (mostly for visible detection purposes), then move forward slightly so all three distance sensors are looking at the same environment, then turn parallel to the nearest wall, then start tracking again.

After the usual number of mistakes and bugfixes, I think I now have a working ‘out-of-range steerval’ recovery algorithm working implemented in WallE3_Complete_V4. Here’s a short video showing the action:

In the above video the robot goes past the end of the left wall at about 6sec, and actually starts to turn left before the ‘out of range’ condition is detected and the robot stops at 7sec. Then the robot moves ahead slowly for 0.5sec to ensure the distance sensors get a ‘clean look’ at the new environment. Then the robot spins very slightly clockwise due to a ‘RotateToParallelOrientation(TRACKING_RIGHT)’ call, and stops again (this is mostly for visual recognition purposes). Starting at 14sec, the robot starts tracking the right-hand wall. Below is the complete telemetry readout from the run:

31 July 2023 Update:

After cleaning up the code a bit, I decided to see how the new ‘excessive steerval’ algorithm handles a 45º break situation set up in my office. The following short video shows the action, followed by the recorded telemetry:

In the video, WallE3 negotiates the wall break without stopping – a result that was unexpected. Looking at the telemetry, I noticed that the steering value never exceeded the 0.99 threshold value for ‘excessive steerval’ detection. The short excerpt from the telemetry (immediately above) shows the time segment from 4.7 to 5.1sec, where the steering value can be seen to range from +0.1 to -0.8 and then back down to -0.1 as WallE3 goes around the break.

I actually think what happened here is the break angle wasn’t actually acute enough to drive the steering value in to the ‘excessive range’.

I made another run with the break angle increased to over 50º, and this did trigger the ‘excessive steerval’ condition. Here’s the video:

In the above video, the break occurs at about 4sec. The telemetry excerpt below shows how the ‘excessive steerval’ algorithm works through the situation, and then continues tracking the left side

02 August 2023 Update:

The ‘excess steering value’ algorithm works, but is not an unalloyed success. Here’s a run where WallE3 appears to negotiate the 50º break OK, but later dives nose-first into the wall – oops:

Here is the telemetry from this run:

Looking through the above telemetry, the ‘ANOMALY_EXCESS_STEER_VAL’ case was detected at 3.9sec (~5sec in video). WallE3 then stopped, performed a 53º CCW turn to parallel the new wall, moved ahead 1/2sec to make sure all left-side distance sensors were ‘seeing’ the new wall, and then started tracking the new wall. However, because WallE3 started from 59cm away, it caused another EXCESS_STEER_VAL anomaly at 10.7sec (~11sec in video). WallE3 again stopped, rotated about 51º CW to (re)parallel the wall, and then continued tracking, starting right at the correct offset of 30cm. At the very end of the run WallE3 ran off the end of the test wall, thus triggering a ‘ANOMALY_OPEN_CORNER’ anomaly.

So, I’m beginning to think that the ‘EXCESS_STEER_VAL’ algorithm might actually be working even better than I thought. I thought I might have to re-implement the ‘offset capture’ phase I had put in earlier and then took out, but this last run indicates that I might not have to.

I made another run, this time starting with WallE3 well outside the offset distance. The video and the telemetry are shown below:

As shown in the above video and telemetry, WallE3 does a good job of approaching and then capturing the desired offset. During the capture phase, the steering value rises from -0.17 to -0.99 (at 0.6sec, almost causing a ‘EXCESS_STEER_VAL’ anomaly detection), decreases to zero at 5.3sec (~3sec in video) and then goes positive with an offset distance of 36.6cm, as shown in the following excerpt:

The above shows that a separate ‘offset capture’ algorithm probably isn’t needed; either the robot will capture the offset without triggering an ‘excess steerval’ anomaly, or it will. If the anomaly is triggered, it will cause the robot to stop, turn to a parallel heading, and then restart tracking – which is pretty much exactly what the previous ‘offset capture’ algorithm did.

05 August 2023 Update:

I may have been a bit premature in saying that WallE3 didn’t need an ‘offset capture’ phase, as I have seen a couple of cases where the robot nose-dived into the opposite wall after trying to respond to an ‘Open Doorway’ condition. It worked before because the procedure was to track the ‘other’ wall at whatever distance the robot was at when the anomaly detection occurred. This obviated the need for an approach maneuver, and thus eliminated that particular opportunity to screw up. However, when I tried to add the constraint of tracking the ‘other’ wall at the desired 30cm offset, bad things happened – oops!

06 August 2023 Update:

I’ve been working on the ‘open corner’ problem, and although I think I have it solved, it isn’t very pretty at the moment. There are some ‘gotchas’ in how and when WallE3 actually updates its distance sensor values, so I think my current solution needs a bit more work. Here’s the video, telemetry and relevant code from a recent ‘open corner’ run in my office.

The video shows the robot stopping after detecting the ‘excessive steering value’ condition. Then it checks for left or right wall availability, and finding none, defaults to the ‘open corner?’ section. This section first commands a 90º deg turn in the direction of the last-tracked wall (left in this case), then moves ahead for 1sec to ensure that all three left-side distance sensors are ‘seeing’ the same wall. Then it calls RotateToParallelOrientation() to take out any initial off-angle orientation, and then calls TrackLeftWallOffset(). The first few times I tried this trick WallE3 just wasn’t cooperating, and when I added some more diagnostics telemetry, I saw that the side distances weren’t updating as I thought they should. I kind of brute-forced the problem by adding a 3-iteration ‘for’ loop with a 200mSec delay to see if there was some sort of latency issue, and this fixed the problem. Here’s the code section and the resulting telemetry:

As can be seen from the above results, the reported distances change dramatically from the first to the second iteration. Don’t quite know why at the moment, but it is definitely something I’ll have to figure out.

07 August 2023 Update:

I modified the code to show the elapsed time in milliseconds rather than decimal seconds to highlight the time differences between distance sensor reads. Here’s the same run with the new times shown:

As can be seen from the above telemetry, the first timed distance printout occurs at 4961mSec, and shows the ‘open corner’ situation (both left and right distances greater than the max tracking distance of 100cm). This first readout occurs after the EXCESS_STEERVAL detection, stop, and subsequent 500mSec ‘skosh’. The second printout is 1mSec later from inside the ‘else //open corner? ‘if’ statement and shows the same distances. Then the robot does the 90º CCW turn and 1sec translation.

Then the next distance readouts both occur at 8046 (about 3 sec later) and are from inside the ‘for’ loop, after a call to ‘UpdateAllDistances()’. This set of 2 readouts still show the ‘open corner’ condition with left/right of 271.3/155.4 cm, and left front, center and rear measurements of 28.4, 271.3 and 325.0 cm respectively. I’m not at all sure why, after the approximately 3sec required for the 90º CCW turn and 1sec translation, that the subsequent UpdateAllDistances() call didn’t return updated measurements.

However, 200mSec later at 8258mSec, the same set of printout does show updated measurements – left/right = 27.4/148.4, left front, center, and rear = 25.1, 27.4, 29.0 cm

Its a mystery!

08 August 2023 Update:

In order to troubleshoot the problem described above where distances didn’t seem to update in a timely manner, I modified the ‘distances only’ feature of the program to print out left/right distances while moving in a straight line between two walls in a ‘V’ shape. The idea is to see whether or not the distances are updating each time the program cycles through the (currently 50mSec) update loop. Any hiccups should show up as ‘flat spots’ in the plot of distances vs time. The run is shown in the short video below, along with an Excel chart of the results:

Then I used Excel’s conditional formatting feature to highlight any cells containing duplicate distance values – the ‘flat spots’ I referred to earlier

As can be seen from the above, there were two sets of duplicates in the left distance column, and three sets in the right distance column. These duplicates could be an artifact of robot speed – i.e. the robot may not be moving fast enough to actually ‘see’ a different distance in the 50mSec between measurements.

I ran the experiment again after bumping the robot’s base speed from MOTOR_SPEED_QTR to MOTOR_SPD_HALF. This time, I got the following:

Hmm, I’m a bit worried about the four duplicate right-side distances at the start, but I think they are a ‘startup artifact’ (at least I hope so). The only other set of dupes is rows 16/17 in the left distance column. All in all, I think everything is working OK.

The differences between the code that produced the above results and the actual code that produced the problem is the actual code performs a ‘Spin Turn’ of 90º followed by a motor run of 1sec between the first and second distance measurements. These two maneuvers take about 4 Sec, so the distance sensors should have updated at least 80 times, and at least 20 times during the 1Sec straight motor run after the 90º turn.

The only other possible difference is the actual code calls ‘UpdateAllDistances()’, while the ‘V Run’ test code uses ‘UpdateAllEnvironmentalParameters()’ (which calls ‘UpdateAllDistances()’ as part of its update process).

09 August 2023 Update:

I changed the line

To

And re-ran the ‘V’ distance test. The results were basically identical as shown below

So, I think it is safe to say that there is no difference in behavior from the use of UpdateAllEnvironmentParameters() and UpdateAllDistances(), which should be a no-brainer as UpdateAllEnvironmentParameters() uses UpdateAllDistances() to update the distances.

Now I’m left with the situation described by Sherlock Holmes – “When you have eliminated all which is impossible then whatever remains, however improbable, must be the truth.“

So, maybe those ‘incidental’ duplicate distance values highlighted in the above runs are actually real? In the first run there were two sets of two dupes on the left side right at the start, and the other two runs there was one set of four dupes (one on the left, one on the right) at the start. If they are real, then that means it took about 100mSec to clear the dupe on the first run, and about 200mSec on the 2nd and 3rd runs. This could be consistent with the behavior shown in the actual ‘open corner’ experiment, where the first measurement after the 1Sec movement was the same as the last measurement before the movement started.

Now I’m beginning to suspect that there is some sort of buffering going on in the VL53L1X distance sensors, maybe due to the recent change to a 50mSec update rate. If the returned measurement was always one (or two?) measurement(s) behind, then that would explain why the measurement(s) received after the 90º turn and subsequent 1Sec motor run was the same as the one(s) before.

I changed the update interval from 50mSec to 75mSec to see if that eliminated the dupes in the ‘V’ run, and got the following:

Inconclusive; there wasn’t a block of four dupes at the start, but this could be just due to physics, as the robot would have moved 50% farther in 75 vs 50mSec during each measurement interval.

I changed my wall configuration back to the ‘open corner’ setup, and made another run using the code that showed the problem before (and with the 3-iteration loop that ‘solved’ it), just to make sure I still had a good baseline.

The baseline run showed the same behavior as before (whew!) so now that I have a solid baseline I can begin to troubleshoot. Again, the code that attempts to solve the problem by looping through ‘UpdateAllDistances()’ is shown below:

The first thing I tried was reducing the delay from 200 to 50mSec. This gave me the following output:

Note that the ‘At top of loop()’ measurement readout of left/right = 197.7/160.9 at 7413mSec and the ‘After RunBothMotors()’ readout of left/right = 197.7/160.9 at 10487mSec are before and after respectively, the SpinTurn() call (approx 2Sec) and the RunBothMotors() call (approx 1Sec). 30.3/29.4/35.8

Hmm, This time the distance report after the very first UpdateAllDistances() call shows reasonable (not great, but reasonable) numbers for left-side front/center/rear distances – 30.6/38.2/49.5. 50mSec later, the report after the 2nd call to UpdateAllDistances() shows 30.3/29.4/35.8. Note that the center and rear distance readings changed quite a bit (about 9 and 14cm respectively) even though the robot wasn’t moving. Clearly something is ‘catching up’ here.

Next I reduced the loop delay() call from 50 to 1mSec and re-ran the experiment, as shown below:

Well, that’s odd! the left-side rear distance is shown as 338.4cm for all three loop iterations! That can’t be right, as the starting front/rear/steerval averages for the orientation turn were 30.04/32.11/-0.21, so now I have no idea what’s going on. I made another run and this time printed out the 10-point average of front/rear distance values performed by RotateToParallelOrientation(). This is what I got:

The run itself was a ‘failure’ in that the robot performed the ‘parallel orientation maneuver’, it made a huge turn in the wrong direction. Looking at the data, it is clear that the culprit here is the first term (215.40cm) in the ‘rear’ 10-point average above. This value comes from the ‘rear’ distance value reported all three times in the 3-iteration loop, utilizing a 1mSec loop delay.

I think what I am seeing here is due to the minimum cycle time of the VL53L1X distance sensors. from the setup code for the VL53L1X sensors I see:

Which I believe means that the minimum time required for a new value to appear in the VL53L1X buffer is 50mSec. For larger distances, the time required might be longer. So that explains why, if the distance reading at the start of my little 3-iteration loop is ‘X’ and the loop time is 1mSec, it is highly likely that all three iterations will report ‘X’. This still doesn’t tell me why the initially reported value is ‘X’ when the robot has moved so that it is at a completely different physical distance from the wall when ‘UpdateAllDistances()’ is called.

Next, I tried breaking the 1Sec ‘move’ into three separate moves, with a call to ‘UpdateAllDistances()’ at the breaks. This shouldn’t make any physical difference, as the moves will all run together. The difference is that the distances reported at the end of the travel should be much more accurate.

Well that trick didn’t work at all! Even with the motor run broken up into 4 250mSec pieces, the errant measurement still showed up as the first element of the 10-element averaging array – bummer!

I made another run, but this time I moved the 200mSec delay in the 10-element averaging loop in front of the measurement, so there would be an initial 200mSec delay before the first element is written.

Aha! This run worked fine, as shown in the telemetry below:

Note that this run was also performed with the 3-element ‘catchup’ loop commented out. Also, it is clear from the above telemetry that the ‘chunked’ motor run wasn’t effective at all, as even the last reported left-side rear distance still shows over 300cm.

It is interesting that the progression from ‘no wall in sight’ to ‘a wall in sight’ can be seen as the robot progresses; the left-side front distance starts at 59.2cm and decreases to the correct value of 27.4cm, while the left-side center distance starts at 286cm, stays the same for the next two measurements, and then decreases dramatically from 296.6 to 46.4cm (still wrong, but better), and the left-side rear distance stays above 300 for all measurements. It is clear from this data that what is being reported is somewhat behind the actual robot position. This may be in part a result of the VL53L1X measurement physics – as the sensor uses a ‘cone’ of light to illuminate the environment and then (I think) computes the histogram of the results. If more of the rear sensor is still ‘seeing’ past the open corner, it will report a ‘no wall’ result.

So, I think we are looking at a two-part problem. The first part is due to sensor physics; the sensor has to ‘see’ the wall throughout it’s FoV (Field of View) cone to produce an accurate measurement. The datasheet shows the default FoV to be 27º, so this is a reasonable conjecture IMHO. The second part is an apparent time lag from the time the FoV changes (in this case – from ‘no wall’ to ‘wall’) to the time the new distance value shows up at the output in response to a measurement request in the code. As I found out from the above experiments, this second issue seems to be completely solved by moving the 200mSec loop delay in the part of the RotateToParallelOrientation() routine that computes a 10-element average to the front of the loop(), so it takes place before the first measurement request. I now believe this 200mSec delay gives the sensors time to ‘catch up’ to the actual environment.

I made a ‘confirmation run’ with the following setup:

  • WALL_TRACK_UPDATE_INTERVAL_MSEC = 50
  • ‘Chunked’ motor run removed – now one 1000mSec run
  • 3-element ‘measurement catchup’ loop replaced by single 200mSec delay followed by a single call to UpdateAllDistances();
  • 200mSec loop delay in RotateToParallelOrientation() 10-element averaging loop moved to top so that it executes before the first measurement request.

Yay! It all worked! Here’s a short video and the relevant telemetry from the run:

Everything looks good with the above results – I think I can now put the ‘open corner’ issue to bed. As a side-benefit, I think I have also improved the function of ‘RotateToParallelOrientation()’, so other calling functions will benefit as well. A quick search of the code shows 7 or 8 places where the function is called.

Help! I’m Spinning out of Control!!

Leave a reply

I have been running ‘field’ (i.e. in my house) tests with WallE3, my autonomous wall-following robot. Unfortunately, WallE3 has demonstrated an unfortunate tendency to lose its mind and start spinning out of control – “around and around the robot goes, and when he stops, nobody knows!”. After a number of trials where this happened, I realized I’m going to have to figure out how to detect this condition so I can get WallE3 to recover properly.

Fortunately, WallE3 knows its relative heading, thanks to its onboard GY-521 MPU-6050 3 Axis Accelerometer/Gyroscope. So, I thought I should be able to detect the ‘spinning’ condition by monitoring the relative heading numbers; if the relative heading values traversed a full 360⁰ within a reasonably short period of time like 3-5 sec, then the robot should be stopped and a recovery algorithm of some sort implemented.

As usual, a seemingly simple algorithm turns out to not be quite as simple as it seems at first glance. The first thing I tried to do was to use my new robot telemetry visualization tool to go back through my recorded telemetry files to find some runs where spinning occurred. Unfortunately, I couldn’t find any – bummer! Not to worry, I decide I could use Excel to ‘invent’ a spinning event by generating a series of monotonically increasing heading values. Then I used Excel and VBA to work out an algorithm for ‘help, I’m spinning’ detection. Shown below is a screenshot of the Excel spreadsheet, and a screenshot of the VBA code that does the detection.

Now to see if this idea actually works ‘in the wild’ (or at least ‘in the robot’)

13 June 2023 Update:

I wanted to capture data from a real ‘spinning’ event to further test the above algorithm, so naturally WallE3 has refused to cooperate, even after several trial runs. So, being the sneaky person I am, I decided to add ‘#define HEADINGS_ONLY’ and associated code section to WallE3’s code base, so I can capture heading date while manually spinning the robot. This worked well, and because it is in a #define block, it gets compiled out for normal operations. After getting that working, I captured a bunch of heading data and dropped it into my Excel setup to see how my VBA code worked with ‘real’ data. As it turned out, this exposed a bug in the algorithm – I had forgotten to handle the case where the cumulative heading is negative, but with a magnitude greater than 360. The fix was to compare the absolute value to 360, as shown in the revised code below:

Stay tuned,

Frank

VL53L1X Distance Measurement Compensation

Leave a reply

Posted 28 April 2023

After replacing all the VL53L0X Time-of-Flight distance sensors on WallE3 with VL53L1X versions with twice the effective range (400cm vs 200cm), I followed the procedure described in this post to derive distance correction expressions for each sensor.

First I set up a distance test range as shown in the following photo:

Distance Calibration ‘Range’

I captured the distance readings from all seven sensors at 10cm intervals from 10 to 60cm, using ‘WallE3_Complete_V2.ino’ in ‘Distances Only’ mode. For each reading I recorded a 20-point average as shown in the following table and plot:

Distance calibration Excel table and plot

This data looks exceptionally good except for the recorded values for the rear sensor. It looks like I may have made a mistake on the 40cm measurement. To investigate this, I re-ran just the rear sensor measurements, resulting in the following table and plot:

Revised plot showing corrected rear distance readings

Looking at the data I suspect I simply forgot to move the robot to the new distance for the 40cm ‘rear’ measurement. I dropped the faulty dataset and then used Excel’s ‘trendline’ feature to show a linear expression fit to the data for each sensor, as shown below:

Distance plots with linear trendline expressions

It turned out to be pretty difficult to select individual sensor plot lines to add trendlines, because (with the exception of the left front sensor) they were all almost identical. In order to add trendlines, I had to use Excel’s ‘Select Data’ feature to show only one line at time. Then when I added the associated trend line, I was able to edit the displayed expression to tag it to the associated sensor, as shown above.

As I did in my previous work with the VL53L0X sensors, I used the trendline expressions to come up with compensation expressions for each sensor, as follows:

  • LF: LFCorr = (LF-3.44)/0.8746
  • LC: LCCorr = (LC-0.7533)/0.9751
  • LR: LRCorr = (LR+0.34)/0.9961
  • RF: RFCorr = (RF+0.4133)/1.0001
  • RC: RCCorr = (RC+0.1867)/0.9877
  • RR: RRCorr = (RR+0.8)/0.9971
  • Rear: RearCorr = (Rear-0.7267)/0.9969

When I plotted the corrected values, I got the following plot:

Compensated VL53L1X sensor data

As can be seen in the above plot, the compensation is almost perfect (for that matter, most of the sensors, with the exception of the LF (Left-Front) sensor, were pretty much spot on without compensation.

With the above information, I modified all seven ‘lidar_XX_Correction()’ functions in ‘Teensy_7VL53L1X_I2C_Slave_V1.ino’ to use the new correction factors, and then made another distance measurement run, with the results shown below:

After adding compensation to WallE3 programs

At this point I believe it is time to declare victory and move on with WallE3 ‘field testing’.

Stay tuned,

Frank