Posted 06 October 2023,

At the very end of the 04 October 2023 Update to the “The Guest Bedroom Problem” post , I made the following statement:

“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!“

After some research on array sorts in C++ I came across this post with a nice example of a quick sort program, which I shamelessly copied. After some fumbling around (including writing my own ‘swap’ routine to allow future porting to Arduino code) I got it to work in my Visual Studio 2022 Community Edition setup with a single int[] array as shown in the following image:

Now the challenge was to extend this algorithm to sort multiple same-sized companion arrays. Looking at the QuickSort code, it appeared all I had to do was duplicate the ‘swap’ operation on all the companion arrays using the same swap indices determined for the ‘master’ array. One additional fly in the ointment was the requirement to handle both int[] and float[] arrays.

First I modified my ‘swap’ routine to be a generic template function as shown below:

Then I renamed the ‘arr’ array from the example to ‘FrontD’ and defined a second ‘Hdg[]’ array of float values with the same length as the original example array as shown below:

Then, for each occurrence of a call to ‘mySwap’ for the ‘FrontD’ array, I added a second call for the ‘Hdg’ array as shown below:

When I ran this code, it *almost* worked right off the bat. Unfortunately the ‘Hdg’ slave array wound up being sorted slightly differently than the ‘FrontD’ master array. After closely examining the code, I finally found the problem. In one place the original programmer used the indexing syntax ‘[i++]’ and ‘[i–]’ as input to the ‘mySwap’ function. This worked fine for the single master array, but failed with the second array because on the second call to ‘mySwap’ the indices had been changed – oops! Here is the original and revised syntax:

Now both calls to mySwap() use the same index values, and life is good. Here’s a debug printout from VS2022 showing a successful program run with a master (int Frontd[]) array and one slave (float Hdg[]) array:

And here is the complete code that produced the above output:

09 October 2023 Update:

After convincing myself that this scheme for synchronizing the sorting of ‘master’ and ‘slave’ arrays, I decided to port the capability into my robot code. I created a new WallE3 program called ‘WallE3_Quicksort_V3’ as a copy of ‘WallE3_Complete_V5’, and then added the ‘quickSort’, ‘partition’, and ‘mySwap’ functions from ‘Quicksort_V3’.

Then I set up a test block in setup() as shown below:

With this setup I got the following output:

In this test, the FrontD[] is the ‘master’ and the ‘Hdg[] is the ‘slave’, and the algorithm is set up to sort the array in increasing order.As can be seen from the above output, the FrontD[] array after Quicksort is indeed sorted from smallest to largest value, and the Hdg[] array elements after Quicksort are ordered in such a way as to correspond to their original relationship to FrontD[].

In my application I want to sort the master array in descending order rather than ascending, so after some googling I found that making the following change:

causes the sort to run in the other direction, giving the following output:

As desired, the ‘FrontD’ master array is sorted in descending order, and the ‘Hdg’ slave array elements are still synchronized with their original FrontD companion elements.

So I changed the test to use real data using the initialization code below:

and got the following output:

Gee, that went well — The FrontD distance array isn’t ordered at all – yuk!

OK, so back to basic troubleshooting. The first thing I did was to replace the FrontD[6] test array in my QuickSortV3 C++ program with the FrontD[36] array of actual front distance values (edited in Notepad++ to be a single line of comma-separated values) to see if I could establish a working baseline – an absolute necessity for efficient troubleshooting.

I had to edit the QuickSortV3 program to remove the references to the second Hdg[](slave) array, as I didn’t want to complicate things, but after I did this, the program sorted FrontD[36] properly in both the forward and reverse direction. To get the reverse sort, I had to flip ‘<=’ to ‘>’ in two places, and ‘>’ to ‘<=’ in one place, as shown below:

The following Excel plot shows the result for both the forward and reverse sorts

Now that I have a working baseline with my QuickSortV3 C++ program, it became easy to see the problem with my WallE3_Quicksort_V3 Arduino program; there are three places that need to be changed for reverse sorts, and I only changed one – ooops! After fixing these problems, I got the following output:

And now, the slave sort seems to be working as well, as shown by the following Excel plot:

The above plot looks very confusing at first, but it seems to be accurate; the ‘before’ picture is straightforward – the robot rotated in 10º steps, so the smooth blue line is expected. The ‘after’ plot looks crazy, but remember it is synchronized with the reverse sorted front distance array, so there is no organizing principal. The relationship of heading values with front distance values after the reverse sort is easier to see with the text output from the program, shown below:

For instance, the largest front distance shown is 507cm (row 9 in the original listing). In the unsorted data, the heading associated with 507cm is 82.1º. In the reverse sorted listing, 507cm is of course on the first row, and so is 82.1º. The lowest front distance (last row of the reverse sorted list) is 24cm, and the heading on the same line is -106.1º. Looking through the original (unsorted) list, 24cm is found on line 26, and the heading on that line is -106.1º as expected.

At this point, it is clear that my plan to have WallE3 turn a full circle while recording front distances and associated headings, then reverse sort the distance data array as the ‘master’ array while maintaining each distance value’s associated heading (the ‘slave’ array) is going to work. Then I should be able to easily find the heading associated with the median front distance of the first(only) group of front distances greater than some threshold – say 1.5m. Looking at the reverse-sorted front distance data above, I see there are about 10 distance measurements above 1.5m as shown below:

The median heading value for this group of 11 distances is 71.4º, which is associated with the front distance value of 444cm.

11 October 2023 Update:

After figuring out how to change my quickSort() function from a forward (increasing values) to a reverse (decreasing values) sort, I decided that I should make it capable of performing either sort (fwd or rev), by adding a boolean parameter to the function signature. I started by going back to my C++ program and making the mods there, and I was able to make it work fairly quickly, as the following output shows:

Then I ported the changes to my Arduino program and got the same results, as shown in the following output:

And here is the test code that produced the above output:

13 October 2023 Update:

After getting this to work in my C++ project, I ported it over to WallE3_QuickSort_V3 and got it working there. Thinking about the overall ‘Guest Bedroom Problem’, it is clear to me that I will need six synchronized arrays – FrontD, Hdg, L/R Dist, L/R Steer. At first I thought I could do this with five calls to ‘QuickSort() – one each for (FrontD, Hdg) and then one each for the remaining four, each using FrontD as the ‘master’ array. However, when I tried this, it failed miserably – only the first sort (FrontD, Hdg) worked, and the remaining four calls did nothing. After thinking about this for a while, I eventually figured out that the first call – (FrontD, Hdg) worked because each time two FrontD array items got swapped in mySwap(), the Hdg array got swapped in the same way – preserving synchrony. However, when the sorted FrontD array was used in the second and subsequent calls, mySwap() never gets called because all FrontD items are already in order. This meant that the second and subsequent ‘slave’ arrays stayed in their original unsorted state – oops!

So the answer to this problem is either keep replacing the ‘master’ parameter to QuickSort() with the unsorted version of FontD[] so that it will get sorted again (causing the required mySwap() calls to the ‘slave’ array, or modify QuickSort to take all five ‘slave’ arrays as parameters. Either way is a real PITA, but I think the ‘all at once’ strategy is more straightforward.

After implementing the ‘all at once’ strategy, I got the following output from my test program:

It appears that both the FWD & REV sorts succeeded (at least with respect to the front distance values). Spot checking the other arrays, we see for front distance values of 23 & 449:

So it is clear that all six arrays are synchronized through both FWD & REV sorts – Yay!

Looking at the reverse sorted and synched data for FrontD values above 1.5m we see a number options for travel directions, as detailed by the following lines from the reverse sort output:

The first four lines above are all ‘left-side’ tracking options. The fifth line above (at FrontD = 240) could actually utilize either the left or right walls for tracking, and the last two are ‘right-side’ options.

The option with the largest ‘head room’ (515cm) is shown in the first line above; on a relative heading of 82.0º, there is 515cm of travel distance available, and the robot is 30.2cm away from the left wall and is oriented almost parallel to it (left steerval is -0.2).

So it looks like this ‘Run To Daylight’ scheme might actually work, but there were a LOT more options than I thought I would have for tracking side and tracking direction. This may have been caused by the fact that I was doing the testing on my lab bench, with lots of ‘clutter’ around. It may be that in a real situation there are very few (or even no) options – we’ll see!

I modified my test program to choose the first acceptable parameter set from the reverse-sorted data, then turn WallE3 to that heading and refresh all parameters. The following short video and the resulting telemetry output are shown below:

As can be seen from the above, the first set of parameters in the synchronized arrays met the criteria, and was chosen. When WallE3 turned to the selected heading and refreshed parameters, everything except the front distance matched very well. I believe the difference in front distances was due to a very slight change in heading which resulted in the distance to a desk chair being measured instead of the distance to the far wall.

Next I tried a test in my office with a simulated corner situation, to see if WallE3 could used his new superpowers for good. I removed the infinite loop at the end of the test program and let loop() run as normal, after setting gl_LastAnomalyCode to ANOMALY_NONE. The following short video and telemetry readout shows the action:

From the telemetry output we can see that WallE3 found an acceptable tracking option at index 1 in the reverse-sorted FrontD array, at a relative heading of 141.4º from the start of rotation, with the following parameters:

Then the robot turned to the desired heading, and then dropped into the normal ‘top of loop’. This caused TrackLeftWallOffset(350.0, 0.0, 20.0, 30) to be called. WallE3 tracked the left wall nicely from 0.7 sec to 3.2 sec where it ran out of wall and detected an EXCESS_STEERVAL Anomaly. All in all, this test seemed to work perfectly.

Stay tuned,

Frank