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:

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