Tag Archives: WallE3

Garmin LIDAR-Lite V4 LED Study

Leave a reply

Posted 16 February 2023

A week or so ago, while finishing up my ‘WallE3_Complete Testing‘ post, I discovered that the reason I (or at least WallE3, my autonomous wall-following robot) was experiencing STUCK_AHEAD errors was because my current forward distance sensor, a Pulsed Light ‘Blue Label’ LIDAR system, couldn’t measure beyond about 4m. Anything beyond that just got reported as 4m (or as a zero, which I converted to 4m). What that meant was that if WallE3 was wall tracking with a lot of open space ahead, it’s sensed distance was a constant 4m, which eventually (eventually being about 5sec) caused my distance variance calculation to go below the ‘stuck’ threshold, and ‘voila!’ STUCK_AHEAD error. I thought about some work-arounds, but there weren’t any good ones that didn’t come with a lot of downside, so I started looking at the new (well, at least new to me) Garmin LIDAR-Lite V4 LED time-of-flight distance sensor. I ordered one from Sparkfun, and it just arrived today – yay! Now to find out if this will really solve my ‘STUCK’ problem. The Garmin LIDAR is significantly smaller and lighter than my current Pulsed Light ‘Blue Label’ LIDAR Lite. I’ve included some photos here for context/scaling:

The first thing I did was put together a small test setup using a plug board and a Teensy 3.5, as shown below:

I started off the study by using one of Sparkfun’s ‘Example1_GetDistance’ program, (modified for better print output) as shown below:

When I ran this program, I noticed that regardless of the delay value in loop(), the time stamps associated with successive distance measurement were a LOT further apart than I expected. So, I fiddled around with the experiment and soon discovered that the time between subsequent measurements varied dramatically depending on the actual distance being measured; short distances were measured very quickly, long distances not so much. In a way this makes a lot of sense (it is a ‘time-of-flight’ device after all), but since we are talking about the speed of light – i.e. 3*10^8 m/sec, it seems a little unreasonable for a measurement of something like 4 m to take significantly longer than for a measurement of 1m, don’t you think?

In any case, I set up an experiment where I pointed the unit at a distant (i.e. 4m) target, and then waved my hand in front of the LIDAR to simulate a close target, and had the program print out the time stamp and the distance measurement. I dumped this into Excel, and plotted the measured distance along with the time difference between subsequent measurements, and got the following plot:

Distance and Differential Time

As can be seen in the above plot the time required between two measurements at a distance of 450cm is approximately 350mSec – ouch! Conversely, the time required between two measurements at a distance of 20cm is approximately 70mSec – a factor of five – wow!

Looking at the code, it is clear that myLIDAR.getDistance() is a blocking function that first triggers a new measurement, and then waits for the LIDAR to finish before reporting the result. Reading through the Garmin documentation, I learned that Garmin developed an Arduino library for the V4 unit. I loaded this library and ran their ‘v4LED/v4LED_fast’ example. Here’s the code (slightly modified for better printout using a Teensy 3.5):

NOTE: the ‘fast’ example sets the number of acquisitions per measurement from the default value of 20 to 0, as shown in the following code snippet from the bottom of setup():

Here’s an Excel plot of the output:

As the above plot shows, this example produces measurements much faster than the first one. Instead of 350mSec at 450cm is approximately, it is about 32Msec – a 10:1 ratio! And instead of approximately 70mSec at a distance of 20cm, it is more like 2Msec – again a 10:1 ratio.

To address the issue of accuracy and repeatability, I used a tape measure to set up a target 100cm away as shown in the following photo, and then moved the LIDAR in 10cm increments toward the target, taking 10 measurements at each stop.

Accuracy/Repeatability Test Setup
Accuracy & Repeatability Plot, with ‘high accuracy’ mode disabled

As the above plot shows, each measurement is repeatable within +/- 2cm. However, the accuracy seemed to deteriorate as the distance decreased. The error started off at about +1 to +2cm at 100cm, and then increased more or less linearly to +8 to +9cm at 10cm.

Next, I re-ran the experiment after commenting out the line myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode that disabled the ‘high accuracy’ mode. However, the results weren’t any better, and actually look worse than before.

Accuracy & Repeatability Plot, with ‘high accuracy’ mode enabled

Just for completeness, I ran the experiment again, with the same configuration as the above plot, except with 100 measurements/position instead of 10. As the following plot shows, repeatability is excellent – accuracy ‘not so much’.

Accuracy & Repeatability Plot, with ‘high accuracy’ mode enabled, 100 meas/position

18 February 2023 Update:

As I was drifting off to sleep last night, I was thinking about what I had learned from that day’s experiments with the Garmin LIDAR V4/LED unit. Something that popped into my head was an image of the last set of data I took, but the inter-measurement delay was about 65-67mSec – not the 1-2mSec I had been seeing – what caused that?

So, today I redid the distance progression measurement, with the ‘high accuracy’ mode enabled as before (except starting at 150cm rather than 100), with the following results:

Accuracy & Repeatability Plot, with ‘high accuracy’ mode enabled, 10 meas/position

This time the time delay between measurements was about 67mSec – the same as I got with that last measurement from yesterday (the image that got stuck in my head as I was going to sleep). In addition, the distance accuracy seemed to be better – the error at 10cm was about +5cm rather than the +8 – +9cm from yesterday, while the error values above 100cm (1m) were negligible.

Redid the above experiment with the ‘high accuracy’ mode disabled (as it was in the original ‘Fast’ measurement code), with the LIDAR at the last position (10cm) from the previous run, with the following results:

Without changing anything else, and with the LIDAR in the same position, I re-compiled the code to re-enable ‘high accuracy’ mode, and got the following result

Wait a minute! The above results are the same as the ‘fast’ results – what gives? I ran this same test several times, recompiling and re-uploading the code each time – no change! Then I cycled power to the unit and tried again, with the following results:

Aha! Recycling the power did the trick – now the results show the lower error (+5 vs +9cm) and the higher delay between measurements (67-68 vs 2 – 3Msec). So the last two plots from yesterday were performed in the ‘fast’ (low accuracy) mode, even after changing (and re-uploading) the Teensy sketch. Now that I think about it – this makes sense, as the change I made from the ‘fast’ configuration to the ‘high accuracy’ one was to comment out the write to register 0XEB. Because I didn’t cycle power to the Garmin, this left register 0XEB’s contents unchanged – e.g. still in ‘fast’ mode! Oops!!!

This is not the first time (by a LOT!!) that my pre-sleep review of the day’s efforts has paid off. Maybe the lack of outside stimulus allows my brain to sift through conflicting or incomplete data without interference. I wonder if this is one of those hidden talents that comes with “The Knack” 🙂

Anyway, now that I have figured out how to get into (and out of) ‘high accuracy’ mode, I re-ran the above experiment (with the unit in ‘high accuracy’ mode) where the LIDAR is aimed at various distances, with the following results:

Distances and measurement times for various distances in ‘High Accuracy’ mode
Distances and measurement times for various distances in ‘Low Accuracy’ mode

From the above it is clear that the ‘high accuracy’ mode is quite expensive in terms of measurement frequency. In ‘high accuracy’ mode measurements can take 500mSec or more for long ranges, whereas in ‘low accuracy’ mode the same measurement takes less than 50mSec even in the worst case.

Next I tried some different values for the number of measurements/point written to Garmin register 0xEB. With values of 10 (0x0A) and 5 (0x05), I got the following responses for medium to long distances:

Distances and measurement times for various distances using 10 acq/meas

Distances and measurement times for various distances using 5 acq/meas

With 5 acquisitions/measurement the measurement time is still well less than 200mSec, and I currently use FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250, so this should be compatible with current operations, even at the longest ranges. I’m not really sure why I care all that much about accuracy, as all I’m trying to do is detect either the ‘STUCK_AHEAD’ condition (when the calculated measurement variance goes below a threshold) and the ‘OBSTACLE_AHEAD’ condition (where the measurement falls below a set distance). Neither of these cases requires much in the way of absolute accuracy.

19 February 2023 Update:

I decided to run Garmin’s ‘Low Power’ example, modified for better printout with the Teensy 3.5 as shown below:

I modified the above to change the inter-measurement delay to 100mSec vs 250Msec, and pointed the LIDAR at various places around (and outside of) the room. Here’s an Excel plot of the results:

Garmin ‘Low Power’ example

As shown, the LIDAR can measure out to at least 7m with a delay time of 100mSec. This right away is a big win, as the Pulsed Light LIDAR needs about 250mSec to measure out to 4m. The rapid fluctuations from about 100cm to about 250cm were caused by me rapidly moving the LIDAR back and forth between targets at these distances. Here’s an enlargement of this area:

Garmin ‘Low Power’ example, rapidly switching between targets

As can be seen in the above enlargement, the LIDAR has no problem following my physical switching between the 100cm & 250cm targets. For the last 7-8 cycles I sped up the switching to see if the Garmin could still follow, and it still did OK. Between 44500 and 46500 (2 sec) there were three cycles, showing that the LIDAR could follow a 250-100cm switching period of about 0.66 sec. This is much faster than WallE3 would need, so this is great!

Current Draw:

The Garmin Operation Manual and Technical Specifications document shows a current drain of 2mA in idle and 85mA during an acquisition. This might well be true when measured with a typical DVM, but the actual current drain is a lot more complicated than that. Here’s a screen grab from my Hanmatek DOS1102 connected to an Adafruit 1NA169 high-side current monitor, configured for 1V/Amp output.

output from 1NA169 while measuring distance for a target approx 6m away
output from 1NA169 while measuring distance for a target approx 5cm away

The maximum output for both cases is about 300mV, corresponding to an instantaneous current drain of about 300mA, while the ‘idle’ current between measurements is about 75mA. However, the duty cycle of these waveforms is quite low, as shown in the next set of screengrabs:

Detail of current waveform while measuring distance to target approx 6m away

As can be seen, the ‘idle’ current is quite low, due to low duty cycle. The ‘acquisition’ current peaks briefly at 300mA and then drops to about 150mA peak, but again this is at a fairly low duty cycle, as shown in the following screengrab

238KHz ‘steady state’ current waveform after 300mA pulse at acquisition start.

This looks like a 238KHz sine wave, but it could be hitting the top end on my ‘scope’. In any case, the average current drain during this period would be something like 75-100mA, which is pretty close to the advertised 85mA ‘acquisition current’.

20 February 2023 Update:

I believe I read somewhere in the docs about placing a smoothing capacitor on the +3.3V line to the Garmin LIDAR, so I did that – placing a 680uF cap on the LIDAR side of the 1NA169 current monitor to see if that smoothed out some of the current pulses. The following plot shows the situation for a far (~6m) target:

current waveform for a far (~6m) target, with a 100mSec delay between measurements

Now the ‘idle’ current is much lower – estimating by eyeball it’s about 25mA, and the acquisition pulse is around 150-250mA. The ‘idle’ waveform has a very low duty cycle, which makes the ‘2mA Idle Current’ figure pretty believable. However, the acquisition current – at least with my unit, appears to be well over 100mA. Here’s the current waveform for a near target (~0.6m)

current waveform for a close (~0.6m) target, with a 100mSec delay between measurements

Interestingly, the measurement time required for the ‘close’ target is about 15mSec, while the time for the ‘far’ target is about 30mSec – a 2:1 ratio, even though he distances themselves are 10:1. In any case, the addition of the capacitor does ‘smooth’ the current waveform considerably, but I would never have noticed any difference if I hadn’t used a current monitor like the 1NA169 .

23 February 2023 Update:

I just now have received a second Garmin LIDAR-Lite V4/LED directly from Garmin, because I wanted a unit without the Sparkfun ‘Qwiic’ breakout board. As part of the project to integrate this new unit into WallE3, I fired up this new device in my little Teensy test circuit. It worked fine, but I noticed a dramatic difference in the current waveform, as reported by the 1NA169 current monitor. The current drain is much lower than the first one I tested. Here’s a screengrab of the current waveform for the new Garmin unit.

New Garmin unit current drain waveform

I guess the take-away from the above results is that the Sparkfun breakout board may be distorting the results from before.

Stay tuned,

Frank

WallE3_Complete Testing

1 Reply

Posted 04 February 2023,

The ‘WallE3_Complete’ program was intended to incorporate all the improvements to my WallE3 autonomous wall tracking robot, as described in this post. At the end of this effort I showed that the ‘Complete_V1’ program did compile, and made a successful run on my ‘two-break’ test wall configuration. Here’s the video that was posted at the end of that previous post:

Successful ‘two-break’ wall run using ‘WallE3_Complete_V1’

This test is just the beginning of the effort to determine how well the ‘WallE3_Complete_V1’ program will perform. There’s lots more testing to come.

The next step is to port the above successful left-side algorithm to the right-side case. After the usual number of mistakes, I got the right-side tracking algorithm going as well, as shown in the following short video.

05 February 2023 Update:

Did my first ‘live’ test with WallE3 this evening, and now I’m wading through the telemetry. Here’s the video of the run:

An Excel plot of the run:

And here is the raw telemetry:

This was a pretty good run. The robot captured (approximately) the desired offset, tracked the wall, negotiated the 45º break, managed to go past a mostly closed door, and then plunged headlong into the next room – oops!

When I started looking at the telemetry data, I realized that I now have too much data – especially the details about how the PID engine is managing. I think I need to seriously reduce the clutter if I want to have any chance at all of understanding WallE3’s behavior during tracking operations. Maybe something like:

I modified the telemetry code in ‘..Complete_V1’ to just output the above parameters.

07 February Update:

Wall tracking seems to be going well at the moment. Not perfect, but OK. Now I’m starting to think about the ‘open door’ problem. This occurs when the robot is tracking a wall down a hallway, and encounters an open doorway on the tracking side – what to do? As it stands, the robot makes an abrupt turn into the open door and may or may not ever return. I’m now thinking that the robot should bypass open doorways if possible. If the other side of the hallway is continuous across the open doorway, then maybe the robot should switch to tracking that wall for the duration of the open doorway (or maybe for as long as that wall lasts?). In order to accomplish this, the robot must be aware of the current distance to the non-tracking side. Currently that information is available, but unused. To investigate this idea I set up a straight wall tracking configuration in my ‘test range’ (aka office) and added a short section of wall on the ‘other’ (non-tracked) side, and instrumented the program as noted above. Here’s the setup:

straight wall on tracked side, wall segment on non-tracked side

And here’s the raw telemetry from the run, and an Excel plot of the left and right side distances:

tracking left wall, with short segment of right wall encountered from 14 to 16.8 sec

As shown above, the right wall distance drops dramatically from around 800 (basically ‘infinite’) to around 50cm during the robot’s transit through this section. It seems reasonable that I should be able to define a new AnomalyCode value – say “OPEN_DOOR” to handle the case where the ‘tracking side’ distance becomes much larger than the ‘non-tracking side’ distance. In this case, the current tracking operation would be cancelled and the main loop would be re-entered from the top, whereupon (one hopes) that the tracking operation would shift to the other wall. When the ‘open door’ section was past, then the ‘off side’ tracking operation could continue, or revert back in some as-yet-to-be-determined fashion.

To start this investigation, I created a new project called ‘WallE3_Complete_V2’ as a clone of ‘WallE3_Complete_V1’ and started from there.

  • I added a new AnomalyCode – OPEN_DOORWAY in ‘enums.h’ and to AnomalyStrArray[]
  • added ‘if else()’ block in CheckForAnomalies() to call new ‘isOpenDoorWay()’ fcn
  • Added ‘isOpenDoorWay(WallTrackingCases trkdir) function to check for this condition
  • Added MAX_TRACKING_DISTANCE_CM = 100; to DISTANCE_MEASUREMENT_SUPPORT region
  • Added OPEN_DOORWAY case to HandleAnomalousConditions()
  • Revised CheckForAnomalies() to accept a TrackingCases parameter, so it can be passed to IsOpenDoorWay(WallTrackingCasestrkdir)

With the above changes, I was able to make a reasonably successful ‘open doorway’ run. Here’s the telemetry showing the robot switching from left-side tracking, to right-side tracking, and then back to left-side tracking.

And here’s a short video showing the action:

10 February 2023 Update:

I thought of another way to ‘improve’ ‘Open Doorway’ handling. I think it would make a smoother transition from one wall to the other by using the current distance to the ‘off’ wall as the desired offset. To do this, the robot will need a global variable to hold the ‘current desired offset’, something like ‘glCurTrackingOffset’, or maybe two of them ‘glCurrentRightTrackingOffset’ and ‘glCurrentLeftTrackingOffset’. The idea would be that when the last AnomalyCode was ‘OPEN_DOORWAY’, then the appropriate non-standard distance would be used the next time through the loop.

Hmm, I guess this means we don’t need the above ‘gl_CurrentLeft/RightTrackingOffset’ variables, but we DO need a global variable to hold the last AnomalyCode, like ‘gl_LastAnomalyCode’. The idea would be that when the side distances are measured at the top of loop() to determine which side the robot should track, the current value of ‘gl_LastAnomalyCode’ will be checked. If it is ‘OPEN_DOORAY’, then TrackLeft/RightWallOffset() will be called using the actual distance to the off wall rather than WALL_OFFSET_TGTDIST_CM, and the ‘gl_LastAnomalyCode’ variable will be set to NO_ANOMALIES.

I made the changes described above, and then after the normal number of screwups, I got a pretty good 3-wall run using the ‘open doorway’ wall configuration shown in the above video. Here’s the telemetry:

In the above telemetry:

  • 12.1 – 16 sec: the robot starts out tracking the left wall at the default offset of 40cm.
  • 16.0 sec: The left distance goes to 785mm and a OPEN_DOORWAY Anomaly code is emitted. This causes the tracking loop to exit and the main loop to run again. This time through, the TrackRightWallOffset function gets called with an offset of 50cm
  • 16.1 – 18.7 sec: The robot tracks the right wall at nominally 50cm
  • 18.7 sec: another OPEN_DOORWAY Anomaly code is emitted. This causes the tracking loop to exit and the main loop to run again. This time through, the TrackLeftWallOffset function gets called, also with an offset of 50cm (I *think* this is a coincidence, but it does look a bit suspicious).
  • 18.8 – 19.6 sec: The left wall is tracked at a nominal 50cm
  • 19.6 sec: The test was terminated.

This looks pretty good, but I had to cheat a little. During my initial runs the robot kept stopping with a STUCK_AHEAD error. When I looked through the telemetry I noticed the front variance value had indeed dropped to near zero, and then I noticed that the front distance measurement itself was holding pretty steady at 400, which is the max the Pulsed Light LIDAR can measure – rats!

There doesn’t seem to be very much I can do to get around the Pulsed Light LIDAR-LITE distance limitation, but this is pretty old technology – almost a decade out of date. Turns out Garmin bought Pulsed Light, and has been marketing the products themselves. Recently they came out with their “V4” version which uses an IR LED instead of a laser, and has a max distance of a whopping 10m! Not only that, but it is just as easy – if not easier – than the original LIDAR Lite to use, and draws much less power – such a deal!

So, I ordered one from Sparkfun, and when it arrives I’ll see if it lives up to the hype and if it will eliminate my false ‘STUCK_AHEAD’ problems

12 March 2023 Update:

After getting my new Garmin LIDAR-Lite V4/LED distance sensor (see this post and this post) tested and integrated into WallE3, my autonomous wall-following robot, I made some more test runs in my office “test range”. As noted above, the problem with my original Pulsed Light LIDAR was that it’s maximum range was about 4m; when the actual distance was greater than 4m, the sensor simply reported ‘400’(cm). This caused the calculated front distance variance to rapidly decrease below the ‘stuck’ threshold, even though in actuality the robot was doing fine. The new sensor, with a maximum range of about 10m should solve this problem. Here’s a recent run on my ‘4m range with an ‘open doorway’ configuration about two-thirds of the way down the track.

Test wall with an ‘open doorway’ about 2/3 of the way down the track

Here’s the telemetry printout from the run:

As can be seen by examining the last four columns of the data (Front, Rear, Front Variance, Rear Variance), the measured front distance starts out at 458cm, well beyond the maximum range of the previous Pulsed Light LIDAR, and the Front Variance stays above 10,000 for all but a handful of readings (the ‘stuck’ threshold is 50). In contrast, the rear distance VL53L0X distance sensor tops out at 200cm, and the data shows that the rear variance does go to zero at the end of the run.

Here’s an Excel plot of the front and rear distances for the first part of the run, just before the robot adjusts to the ‘open doorway’ anomaly:

Front/Rear distances up to the ‘open doorway’ segment

Here’s a plot of the front and rear variances for the same segment:

Front and Rear variance values up to the ‘open doorway’ segment

As can be seen in the above plot, the front variance value stays above 10,000 for the entire portion up to the ‘open doorway’ segment, showing that the new Garmin LIDAR sensor is doing it’s job very nicely.

The next plot shows what happens when the robot reaches the ‘open doorway’ segment. The robot is happily tracking the left wall at a nominal 40cm offset when the left distance goes to 200cm (max sensor range) and the right distance drops to below 50cm – essentially the two side distance measurements trade places). This is the definition of the ‘open doorway’ condition, and this should cause the robot to start tracking the right wall instead of the left.

Left/Right distances up to the ‘open doorway’ segment

The next plot below is the same left/right distance plot, but during the ‘open doorway’ segment.

Left/Right distances during the ‘open doorway’ segment

The robot starts tracking the right-hand wall at about 19,100mSec, and this condition persists to about 19,800mSec, where the left and right distances switch again, and the robot starts tracking the left-hand wall again, as shown in the following plot:

Left/Right distances after the ‘open doorway’ segment

When all three segments are stitched together, you get the following plot:

Left/Right distances all three segments

The ‘open doorway’ segment centered around 19,200Msec is clearly visible, as the left and right distances switch.

Here’s a short video showing the entire run:

New Garmin LIDAR-Lite V4/LED, 4m test wall with ‘open doorway

Stay tuned!

The way forward from here

1 Reply

Posted 31 January 2023

I think I have now arrived at the point where the major sub-systems in my anonymous wall-following robot program are working well now.  The last piece (I think) of the puzzle was the offset tracking algorithm (see https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/).

  • Wall-Track Tuning – just finished (I hope)
  • Move to Front/Rear/Left/Right Distance – These are generally working now.
  • Detection of and tracking/homing to a charging station via IR beam.
  • Error condition detection/handling – this is still an open question to me.  The program handles a number of error conditions, but I’m sure there are some error conditions that it doesn’t handle, or doesn’t handle well (the ‘open doorway’ detection and handling issue, for one).

The last full program I see in my Arduino directory is ‘WallE3_WallTrack_V5’, although it appears that _V5 isn’t that much different than _V4. Below are the changes from V4 to V5:

  • There are a number of changes in _V5’s #pragma OFFSET_CAPTURE section, but as I just discovered in this post, the new wall tracking configuration with PID(350,0,0) and ‘tweak’ divisor 50 (as described at the bottom of this post) means that I don’t need a separate ‘offset capture’ feature at all – the normal offset tracking routine handles the capture portion quite well, thankyou.
  • There are some very minor changes in _V5’s TrackLeftWallOffset(), but this section will be replaced in its entirety with the algorithm from the above post
  • V5 has a function called OrientCorr() that doesn’t exist in _V4, but it was just for debugging support.

So, I think I will start by creating yet another Arduino project from scratch, with the intention of building up to a complete working program, with wall offset tracking, charging station detection/docking, and anomalous condition detection/handling. But what to call it? I think I will go with ‘WallE3_Complete_V1’ and see how that works. I think I will need to be careful during it’s construction, as I want to incorporate all the progress I have made in the various ‘part-task’ programs:

  • WallE3_AnomalyRecovery_V1/V2
  • WallE3_ChargingStn_V1/V2/V3
  • WallE3_FrontBackMotionTuning_V1
  • WallE3_ParallelFind_V1
  • WallE3_RollingTurn_V1
  • WallE3_SpinTurnTuning_V2
  • WallE3_WallTrack_V1-V5
  • WallE3_WallTrackTuning_V1-V5

I will start by creating WallE3_Complete_V1 as a new blank program, and then going carefully through each of the above ‘part-task’ programs (in alphabetical order each time, just to reduce the confusion factor) to pull in the relevant bits.

Includes:

It looks like the complete #includes section is:

Oddly though, many of my programs don’t #include <wire.h>, but they compile and run fine – no idea why. OK, the reason is – “I2C_Anything.h” also includes <wire.h>. When I look at ‘wire.h’, I see it has a ‘#ifndef TwoWire_h’ statement at the top, so adding #include <wire.h> at the top won’t cause a problem, and I like it just for its informational value.

After copying in the #includes, I right-clicked on the project name and selected ‘add->existing item…’ and added ‘enums.h’, ‘FlashTxx.h’ and ‘FlashTxx.cpp’ from the ‘…\Robot Common Files’ folder. Then I opened a CMD window and used mklink (see this link) to create hard links to ‘board.txt’ and ‘TeensyOTA1.ttl’. At this point, the minimalist program (only #defines and empty, setup() and loop() functions) compiles without error – yay!

#Define Section:

Next in line are all the #Defines:

TIME INTERVALS Section:

Note that the above const declarations for MSEC_PER_DIST_UPDATE and FRONT_DISTANCE_UPDATE_INTERVAL_MSEC could just as easily be in the DISTANCE_MEASUREMENT_SUPPORT section, but I decided to try and keep all the timing stuff together. I’ll also put these declarations in the DISTANCE_MEASUREMENT_SUPPORT section but commented out with a pointer to the timing section.

TELEMETRYSTRINGS Section:

I removed the “_Capture” elements from TrkStrArray[] as these are no longer needed (wall offset capture now just part of TrackLeftRightOffset()). The rest should be OK for now.

DISTANCE_MEASUREMENT_SUPPORT Section:

Copied this from ‘WallE3_AnomalyRecovery_V2’. It looks pretty complete. Note that I left the ‘STUCK_FORWARD/BACKUP_TIME_MSEC’ declarations here rather than in the ‘Timing’ section. Didn’t seem to warrant the attention.

PIN ASSIGNMENTS Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks pretty complete

MOVE TO DESIRED DISTANCE Section:

I changed the #pragma name from ‘FRONT_BACK OFFSET MOTION PID’ to ‘MOVE TO DESIRED DIST SUPPORT’ as that is a better description of what these parameters do. In addition, the ‘Offsetxxxx’ name is no longer relevant – it should be changed to ‘MoveToDistxxx’, but I don’t want to do that willy-nilly now. I’ll wait until the entire program will compile, and then (after one last check) I’ll make the change globally.

Charge Support Parameters Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

MOTOR_PARAMETERS Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

MPU6050_SUPPORT Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

WALL_FOLLOW_SUPPORT Section:

Copied from ‘WallE3_AnomalyRecovery_V2’, except I commented out the ‘LEFT/RIGHT_WALL_PARALLEL_STEER_VALUEs as these are no longer used.

HEADING_AND_RATE_BASED_TURN_PARAMETERS Section:

Only the ‘TurnRate_Kx’ parameters, the ‘HDG_NEAR_MATCH’, HDG_FULL_MATCH, ‘HDG_MIN_MATCH’ and ‘DEFAULT_TURN_RATE’ should be in this section. Everything else should be local to the ‘turn’ functions (I kept the ‘Prev_HdgDeg’ and ‘TurnRatePIDOutput’ at global scope for now to avoid lots of compile errors, but they should also be removed.

PARALLEL_FIND_SUPPORT Section:

03 February 2023: The entire PARALLEL_FIND_SUPPORT section has been removed, as the new RotateToParallelOrientation() function no longer uses a PID engine

IR_HOMING_SUPPORT Section:

Copied these from WallE3_AnomalyRecovery_V2. At some point the ‘IRHomingSetpoint’ variable should be changed from global to local scope, and ‘IRHomingLRSteeringVal’ should be renamed to ‘gl_IRHomingLRSteeringVal’ to show global scope. Same with ‘IRFinalValue1’, ‘IRFinalValue2’ and ‘IRHomingValTotalAvg’

GLOBAL_VARIABLES Section:

It looks like most, if not all, of the global variables associated with TrackingCases, OpModes, and TrackingStates are no longer used. I left them in for now, but will go back through and remove unused vars when WallE3_Complete_V1 is finished.

SETUP():

SERIAL_PORTS Section:

Copied verbatim from ‘WallE3_AnomalyRecovery_V2’ – looks good

PIN_INITIALIZATION, SERIAL_PORTS, I2C_PORTS, MPU6050, VL53L0X_TEENSY Sections:

Copied all these verbatim from ‘WallE3_AnomalyRecovery_V2’ – these haven’t changed in literally years, so shouldn’t be an issue

LR_FRONT DISTANCE ARRAYS, #IFDEF DISTANCES_ONLY, IRDET_TEENSY, #IFDEF IR_HOMING_ONLY, IR_BEAM_STEERVAL_ARRAY, POST_CHECKS Sections:

Copied all these verbatim from ‘WallE3_AnomalyRecovery_V2’ – these haven’t changed in literally years, so shouldn’t be an issue. I did note, however, that the ‘POST_CHECKS’ section always runs as the ‘NO_POST’ #define isn’t used. Will leave as it is for now. Thinking a bit more about this – it seems that what I originally thought would be a potentially long, onerous, and not very useful POST hasn’t turned out that way. It is long and onerous, but very necessary, as it initializes and connects to all the peripheral equipment. So I think I will simply remove the #define NO_POST line, and rename the section that ‘ripples’ the rear LED’s from ‘#pragma POST_CHECKS’ to ‘#pragma FLASH_REAR_LEDS’

This complete the ‘setup()’ function – on to ‘loop()’!

Obviously, the contents of the loop() function varies widely across all the ‘part-task’ programs, so this will probably require a lot of work to ‘harmonize’ all the part-task features into a complete program. I think I will try to go through the ‘part-task’ programs in alpha order to see if I can pick out the salient features that should be included.

WallE3_AnomalyRecovery_V2:

loop() has just three main sections – IR_HOMING, CHARGING, and WALL_TRACKING. The first two above are specific operations associated with homing and connecting to the charging station, and the last one is very simple – it just calls either TrackRightWallOffset() or TrackLeftWallOffset().

WallE3_ChargingStn_V2:

This one has the same three sections as WallE3_AnomalyRecovery_V2 and AFAICT, they are identical.

WallE3_FrontBackMotionTuning_V1:

In addition to the same three sections as WallE3_AnomalyRecovery_V2, this one has ‘PARAMETER CAPTURE’ and ‘FRONT_BACK_MOTION_TEST’ sections. The ‘PARAMETER CAPTURE’ section captures test parameter value input from the user, and the ‘FRONT_BACK_MOTION_TEST’ section actually performs the motion with the user-entered parameters. So, we need to make sure that the actual functions used for testing are copied over and the global front/back motion PID values as well. From the ‘Move to a Specified Distance’ post, I see that the final PID values are (1.5, 0.1, 0.2), and these values were incorporated into the ‘WallE3_WallTrackTuning_V5’ as follows:

Uh-Oh, trouble ahead! When I looked at the OffsetDistKx values copied into _Complete_V1 from WallE3_AnomalyRecovery_V2, I see:

so, which set of values is correct? The WallE3_AnomalyRecovery_V2 project was created 9/26/22, while the Move to a Specified Distance, Revisited post is dated 24 December 2022, so much more recent. We’ll at least start with the later PID values of (1.5,0.1,0.2)

Copied the following functions from WallE3_FrontBackMotionTuning_V1 into WallE3_Complete_V1:

  • bool MoveToDesiredFrontDistCm(uint16_t offsetCm)
  • bool MoveToDesiredLeftDistCm(uint16_t offsetCm)
  • bool MoveToDesiredRightDistCm(uint16_t offsetCm)
  • bool MoveToDesiredRearDistCm(uint16_t offsetCm)

WallE3_ParallelFind_V1:

This program has the same structure in setup – with a PARAMETER CAPTURE section followed by the actual test code, all in setup(). However, when I tested this for functionality, I realized it a) only addressed the ‘left wall tracking’ case, and b) didn’t work even for that case.

So, I spent some quality time with this ‘part-task’ program and got it working fairly well, for both cases. See this post for the details and testing results.

After getting everything working, I copied the RotateToParallelOrientation() function from ‘WallE3_ChargingStn_V2’ into ‘WallE3_Complete_V1’ (in the WALL_TRACK_SUPPORT section) and then replaced the actual code with the code from the latest ‘WallE3_ParallelFind_V1’ part-test program.

After making the copy, there were a number of compile errors due to needed utility functions not being present. From ‘WallE3_ChargingStn_V2’ I copied in the following functions:

  • Entire HDG_BASED_TURN_SUPPORT section
  • Entire DISTANCE_MEASUREMENT_SUPPORT section
  • Entire MOTOR_SUPPORT section
  • Entire IR_HOMING_SUPPORT section
  • Entire VL53L0X_SUPPORT section
  • UpdateAllEnvironmentParameters()
  • Entire CHARGE_SUPPORT_FUNCTIONS section
  • copied ‘float glLeftCentCorrCm;’ from WallE3_ParallelFind_V1
  • IsStuckAhead(), IsStuckBehind(), IsIRBeamAvail(), GetWallOrientDeg(), CorrDistForOrient()

At this point the program still doesn’t compile, but I am going to stop and continue with looking at each part-task program in order, and then I’ll come back to the task of getting ‘Complete’ to compile

WallE3_RollingTurn_V1:

Based on the results described in ‘WallE3 Rolling Turn, Revisited’, I copied the ‘RollingTurn()’ function verbatim into ‘Complete’, and also added the ‘TURN_RATE_UPDATE_INTERVAL_MSEC’ constant to the ‘TIME INTERVALS’ section.

WallE3_SpinTurnTuning_V2:

Based on the ‘WallE3 Spin Turn, Revisited’ post, it looks like the original ‘SpinTurn()’ function is unaffected, but with a different set of PID values. The ‘final’ PID value set is (0.7,0.3,0). I did a file compare of the SpinTurn() functions between the SpinTurnTuning_V2 and ChargingStn_V2 programs, and found that they are functionally identical (ChargingStn_V2 uses TeePrint, and SpinTurnTuning_V2 uses gl_SerPort). So, I copied the SpinTurnTuning_V2 versions of both the SpinTurn() functions (one with and one without Kp/Ki/Kd as input parameters) to ‘Complete_V1’, and copied the Kp,Ki, & Kd values from SpinTurnTuning to ‘Complete_V1’.

WallE3_WallTrack_V1-V5:

From this post I see the following changes through ‘WallE3_WallTrack_V1-V5 series of programs:

WallE3_WallTrack_V2 vs WallE3_WallTrack_V1 (Created: 2/19/2022)

  • V2 moved all inline tracking code into TrackLeft/RightWallOffset() functions (later ported back into V1 – don’t know why)
  • V2 changed all ‘double’ declarations to ‘float’ due to change from Mega2560 to T3.5

WallE3_WallTrack_V3 vs WallE3_WallTrack_V2 (Created: 2/22/2022)

  • V3 Chg left/right/rear dists from mm to cm
  • V3 Concentrated all environmental updates into UpdateAllEnvironmentParameters();
  • V3 No longer using GetOpMode()

WallE3_WallTrack_V4 vs WallE3_WallTrack_V3 (Created: 3/25/2022)

  • V4 Added ‘RollingForwardTurn() function

WallE3_WallTrack_V5 vs WallE3_WallTrack_V4 (Created: 3/25/2022)

  • No real changes between V5 & V4

I *think* that I copied most pieces in from WallE3_WallTrack_V2, so I’m going to go back through the entire program, looking for V2-V5 differences.

Loop():

I think I have everything above loop() accounted for. Now to try and make some sense of loop(). I need to be cognizant of ‘WallE3_ChargingStn_V2’, ‘WallE3_WallTrack_V5’ and ‘WallE3_AnomalyRecovery_V2’ versions of ‘loop()’. Going through all the programs, I see the following differences in loop().

  • ChargingStn_V2 adds ‘UpdateAllEnvironmentParameters()’ compared to WallE3_WallTrack_V5.
  • WallE3_AnomalyRecovery_V2 also adds ‘UpdateAllEnvironmentParameters()’ and in addition changes all ‘myTeePrint.’ to ‘gl_SerPort->’ compared to ChargingStn_V2. So, I copied the WallE3_AnomalyRecovery_V2 versions of IR_HOMING, CHARGING, and WALL_TRACKING into Compare’s loop() function.

So, at this point I have all of the ‘pre-setup’, setup(), and loop() stuff in properly (I hope). This should compile, but probably won’t, so I’ll need to go through the PITA part of figuring out why, and fixing it – oh well.

I got IR_HOMING and CHARGING working (well, at least compiling), and now I’m working on WALL_TRACKING. For this section I need to decide which version of TrackRight/LeftWallOffset to use (or at least start with).

WallE3_WallTrackTuning_V5 ended up with a very successful setup with PID(350,0,0) and offset divisor of 50, but it only addressed left-side wall tracking, and didn’t use the actual ‘TrackLeftWallOffset()’ function. So first we need to move the test code into ‘TrackLeftWallOffset()’, and then port ‘TrackLeftWallOffset()’ into ‘TrackRightWallOffset()’.

Here’s the tracking code from WallE3_WallTrackTuning_V5:

Comparing the above to TrackLeftWallOffset() from WallE3_WallTrack_V5….

  • WallE3_WallTrack_V5 has an extra ‘float spinRateDPS = 30;’ line for use in it’s now unneeded ‘OFFSET_CAPTURE’ section.
  • WallE3_WallTrackTuning_V5 adds ‘MsecSinceLastFrontDistUpdate = 0;’ and I think this is needed for the ‘final’ version.
  • WallE3_WallTrackTuning_V5 adds ‘&& !gl_bIRBeamAvail’ to the initial ‘while()’ statement
  • WallE3_WallTrackTuning_V5 computes the ‘offset_factor’ and adds it to WallTrackSteerVal.
  • The rest of the function is essentially the same, but WallE3_WallTrackTuning_V5 uses a custom inline telemetry output section rather than ‘PrintWallFollowTelemetry();’
  • WallE3_WallTrackTuning_V5 doesn’t have the line ‘digitalToggle(DURATION_MEASUREMENT_PIN2);’ for debug purposes – I should probably keep this.
  • WallE3_WallTrackTuning_V5 doesn’t have ‘HandleAnomalousConditions(errcode, TRACKING_LEFT);’ at the end. This should be kept as well.
  • WallE3_WallTrack_V5 uses ‘myTeePrint.’ instead of ‘gl_SerPort->’

So, I copied ‘TrackLeftWallOffset()’ to Complete_V1 and made the above changes. I only copied in the ‘parameterized’ version of ‘TrackLeftWallOffset()’ – the ‘parameterless’ version is no longer needed.

  • Removed ‘float spinRateDPS = 30;’
  • added ‘MsecSinceLastFrontDistUpdate = 0;’
  • Removed entire OFFSET_CAPTURE section
  • added ‘&& !gl_bIRBeamAvail’ to the initial ‘while()’ statement
  • Edited the PIDCalcs line to match WallTrackTuning_V5, except with ‘kp,ki,kd’ symbols instead of ‘WallTrack_Kp, WallTrack_Ki, WallTrack_Kd’
    • Replaced ‘PrintWallFollowTelemetry() with custom telemetry printout
  • kept ‘digitalToggle(DURATION_MEASUREMENT_PIN2);’ for debug purposes
  • ‘HandleAnomalousConditions(errcode, TRACKING_LEFT);’ at the end, and copied in the function code from WallE3_WallTrack_V5.

After finishing all this up and adding some required MISCELLANEOUS section functions, the ‘Complete_V1’ program compiles with only one error, as follows:

This error is due to the fact that I haven’t yet ported the WallE3_WallTrackTuning_V5 code to the ‘right side wall’ case. I think I’ll comment this out for now, until I can confirm that the robot can actually track the left side wall.

04 February 2023 Update:

Well, what a miracle! I commented out the call to ‘TrackRightWallOffset’, compiled the program, ran the left-side tracking test on my ‘two-break’ wall configuration, and it actually worked – YAY!! Here’s a short video showing the run.

Stay tuned!

Frank

WallE3 Wall Tracking, Revisited

Leave a reply

Posted 12/23/22

In the last couple of months I have made some significant improvements in WallE3’s capabilities. I started by completely re-doing the compensation algorithms for the seven ST Micro’s VL53L0X time-of-flight distance sensors (two each 3-element side-looking arrays and one rear-looking distance sensor). I followed this with improvements to both the ‘spin turn’ and ‘rolling turn’ features.

Next, I went back through my ‘MoveToDesiredLeft/Right/Front/RearDistCm()’ family of subroutines and made sure they were all working properly now with the much more accurate distance compensation algorithms. One interesting thing that came out of this effort was the realization that shorter measurement intervals (i.e. 50mSec vs 200mSec) produced an unintended side-effect of making ‘Stuck’ detections much more prevalent. This occurs because the appropriate (front or rear) 50-element distance array fills up much faster at 50mSec/measurement than it does at 200mSec/measurement, so identical (or nearly identical measurements will cause a stuck detection earlier (5 measurements/sec means a 50 element array will fill in 10 sec but 20meas/sec fills the array in 2.5sec. When I used 50mSec/meas in the ‘MoveToDesired…()’ routines, the robot would often exit the routine with a ‘stuck’ error code as it slowed down approaching the desired distance condition. These functions do fine with a more coarse time interval (eliminating the ‘stuck’ declarations), so I went back to 200mSec/measurement.

Now I am going to try to incorporate the above improvements into my previous wall track testing program, ‘WallE3_WallTrackTuning_V4’. As usual, I will start by creating ‘WallE3_WallTrackTuning_V5’ as a clone of ‘_V4’ and start making changes from there.

WallE3_WallTrackTuning_V5:

I am going to try and make WallE3_WallTrackTuning_V5 as ‘clean’ as possible, removing as much ‘dead’ code as possible and consolidating things like sensor measurement intervals.

Timing intervals:

Searching through the code for ‘elapsedMillis’ objects, I see the following global declarations:

Then I did a search for “MSEC” all upper case and found:

The front LIDAR sensor starts to generate errors for long distance measurements when the measurement interval falls below 200mSec

The VL53L0X time-of-flight sensors need a ‘measurement time’ of 50mSec or greater. This is handled by the VL53L0X array Teensy, but it means that UpdateAllEnvironmentParameters() shouldn’t be called more frequently than 20HZ.

The MPU6050 can support an update interval of 30mSec or greater, and this time is used for all turning operations.

Telemetry readouts should occur no more than once every 200mSec.

MoveToDesiredFront/Back/Left/RightDistCm()

Based on my recent work on these functions, it looks like PID = (1.5, 0.1, 0.2) will work for all cases, so all I have to do is modify the existing ‘OffsetDistKp/Ki/Kd’ values. Note that in my testing these were parameters to the function call instead of program constants, but now I can go back to just having the desired offset as the only parameter.

So, I copied each of the above functions to WallE3_WallTrackTuning_V5 from WallE3_FrontBackMotionTuning_V1, removed Kp,Ki,Kd from the sig, and replaced all occurrences with OffsetDistKp/Ki/Kd. I also ported the CorrDistForOrient() function, as it is required by the MoveToDesiredLeft/Right() versions

01 January 2023 Update:

After getting the ‘MoveTo…’ functions working, I discovered that ‘RotateToParallelOrientation()’ didn’t work well at all, and in fact found a note from my former self that the function was ‘fatally flawed’ – oops! So, I revisited my ‘WallE3_ParallelFind_V1’ part-task project to see if I could get it to work better now that VL53L0X distances are being reported as float vs integer objects, and after what I hope is much better sensor error compensation. As shown in this post, RotateToParallelOrientation() now works much better, albeit somewhat slowly, with PID = (20,4,0).

Offset Capture with ‘RotateToParallelOrientation’ ‘at end

02 January 2023 Update:

Starting to make some full-up left wall tracking runs, using the updated code from earlier work. In particular, I am trying to see if my older idea about combining an offset-driven steering angle modifier for the PID tracking algorithm will work. The ‘offset_factor’ incorporates the distance error into the reported steering angle, which in turn is used in the PID machine to drive the combined steering angle to 0.

Here’s an early run:

This worked, ‘sorta’. Part of the problem with this run is the robot’s orientation with respect to the wall at the start of the run. This is supposed to be parallel with the wall, but it obviously isn’t, and I don’t know why. Here’s the data from the ‘RotateToParallelOrientation()’ step

This certainly looks good – with a front/rear distance difference of only 0.3cm, and a steering value of 0.02. However, as shown in the following screengrab of the above video, the robot’s orientation just after the parallel find operation is anything but parallel

movie frame grab just after ‘RotateToParallelOrientation()’

I re-instrumented the ‘RotateToParallelOrientation’ function to print out 10 sets of front/back distances directly after completing it’s ‘ParallelFind’ operation, and made another run. The photo below shows the ending orientation, followed by the data

Robot orientation immediately after ‘RotateToParallelOrientation()’

According to the photo, the robot is definitely not oriented parallel to the wall. However, according to the telemetry data, it is (44.4 front, 44.2 rear, steerval = 0.02). Even curioser, the actual physical measurements taken using a tape measure show that the front/rear distances are about 47/44cm, or a steerval of about 0.3! Something is definitely wrong here.

Uncommented the #DISTANCES_ONLY define and re-ran, with the robot position/orientation unchanged:

In the above data, the front distance varies from 42.6 to 44.6cm with an average of 43.7cm, and the rear distance varies from 44.1 to 45.5cm with an average of 44.8cm.

So the program thinks the front/back distances are closer together than the tape measure does (44.5/45.0 vs 47/44). This is a pretty big discrepancy. Rotating the robot to be physically parallel with front/rear distances = 40cm, I get:

When the robot is physically parallel, the reported front distance varies from 38.2 to 39.2cm with an average of 38.8cm, and the rear distance varies from 36.7 to 38.3cm with an average of 37.9. The left steering value varies from 0.02 (38.8/38.1) to 0.14 (38.9/37.5) with an average of 0.09.

Well, it looks like the average reported distances and steering values are pretty close to reality, so maybe my original calibration efforts aren’t entirely screwed up. However, it is abundantly clear at this point that my current ‘RotateToParallelOrientation()’ algorithm isn’t reliable, due to very noisy distance value measurements.

01/09/23 Update:

After getting ‘RotateToParallelOrientation()’ working better (now it just uses the array front/rear distance measurements to calculate the off-parallel angle, and then does a ‘SpinTurn’ by that amount), I resumed the effort (see the 02 January Update above) trying to determine if my older algorithm for combining the raw steering value with an ‘offset adjustment factor’ based on the robot’s distance from the desired offset distance would now work better given the improvements I have made in VL53L0X sensor error compensation and off-parallel distance measurement compensation.

As it turns out, the answer seems to be ‘no’. After a multitude of runs with my test wall set up for two 30-45deg ‘breaks’, I couldn’t find any set of PID values that would allow the robot to track the wall – it always either took off for parts unknown, or crashed into the wall at some point.

So, back to the original algorithm of using the wall offset distance directly in the PID engine.

11 January 2023 Update:

I’m confused – not an unusual state for me to be in – but still…..

After all the above improvements, I still was unable to produce reasonable tracking performance using either the steering value or the offset distance as the parameter to be controlled. And, even more confusing, I have an entire post dedicated to demonstrating successful wall tracking using the orientation-angle-corrected distance to the wall as the input to the PID engine, with the desired wall offset as the setpoint, as shown here:

With this algorithm, I settled on PID(3,0,1) as the best parameter set, with the result shown in this short video (copied from the above post):

Wall tracking using corrected distance measure as input, and desired offset as the set-point

And then, I have another post demonstrating that using the steering value as input and 0 as the setpoint also works, as shown in this short video with PID(300,0,300)

Right-side wall tracking using steering value as input with PID = (300,0,300)

Here’s the data and short video from a run on my longer ‘4 meter’ test range with two 30º breaks:

Using steerval only. Note monotonically decreasing distance

Even more confusing, it appears that the earlier (September 2021) trial using the steering value input also used the measured center distance to modulate the steering value so the robot would tend to track the steering value but also trend toward the desired wall offset distance. Here’s the tracking code from FourWD_WallTrackTest_V3:

In the above code snippet, ‘Lidar_RightCenter’ is in mm, so WALL_OFFSET_TGTDIST_CM must be multiplied by 10 to match units.

At this point I am thoroughly confused, (but hopeful, since I have evidence from an earlier version of myself that something (actually two somethings) actually work. I believe the next step is to see if I can use my WallE3_WallTrackTuning_V5 code to consolidate everything down to something that works.

12 January 2023 Update:

I went back and loaded up WallE3_WallTrack_V2.ino and ran it on my 4m ‘range’ with two 30º breaks. The robot tracked amazingly well, as shown in the following telemetry output and Excel plot

WallE3_WallTrack_V2’s tracking algorithm uses the difference between the desired and measured offset distances to ‘tweak’ the steering value, as discussed above, so clearly this works – or at least doesn’t screw things up too badly. In the above telemetry output, the ‘Steer’ column is the steering value after the offset distance adjustment shown here

So at the point where the robot hit the minimum center distance of about 154mm, the steering value adjustment would be (154-400)/1000 = -0.246. The total steering value term at this point was 0.23, which means the ‘raw’ steering value was +0.016 and the offset distance error term accounted for ~97% of the total. This is good evidence that including the the distance offset term works.

My new new new plan is to focus on my October 2022 post that uses the orientation angle corrected offset distance as the input to the PID engine, and see if I can incorporate this, along with all my recent updates/bugfixes into WallE3_WallTrackTuning_V5

WallE3 Spin Turn, Revisited

1 Reply

Posted 23 December, 2022

Just over a year ago I made a radical change to WallE2’s form factor to improve it’s turning performance. Since then I have been making other changes and improvements to WallE3, including a recent successful effort to improve WallE3’s ‘rolling turn’ capabilities. Based on that, this post describes a similar effort to improve WallE3’s ‘spin turn performance.

To date, WallE3’s ‘spin turn’ performance has been lackluster at best. Rather than turning smoothly, it tends to ‘motorboat’ it’s way through the turn, starting and stopping several times through the turn. After achieving some success with smoothing out the ‘rolling turn’ feature, I decided to take another crack at ‘spin turn’.

I started by creating yet another part-task program that does nothing but support spin turn operations. This program accepts a set of parameters, calls the existing ‘SpinTurn()’ subroutine to execute the specified spin turn, and then returns to the parameter entry state for another go.

The starting point for the effort was a 90º CCW turn at 45º/s, with PID = (1,0,0). As shown below, this produced a nice smooth turn, but way too slow (17º/s vs 45º/s):

After a coupe of dozen trials, I gradually worked my way up to a ‘final’ PID of (0.7,0.3,0). Here are plots and videos for various turn lengths and rates.

SpinTurn(CCW,30,45)PID(0.7,0.3,0)

SpinTurn(CCW,60,45)PID(0.7,0.3,0)

SpinTurn(CCW,90,45)PID(0.7,0.3,0)

After running these tests, I decided to see how WallE3 would perform on carpet with these same settings. As shown below, carpet performance was pretty much identical to the smooth surface runs.

SpinTurn(CCW,30,45)PID(0.7,0.3,0)
SpinTurn(CCW,60,45)PID(0.7,0.3,0)
SpinTurn(CCW,90,45)PID(0.7,0.3,0)

So, with much better performance now for both the ‘rolling turn’ and ‘spin turn’ features, it is now probably time to revisit the whole wall-tracking thing, and see if I can achieve better performance than in past efforts.

Stay tuned,

Frank

WallE3 Rolling Turn, Revisited

2 Replies

Posted 04 December 2022,

A few days ago I described my revisit to WallE3’s ‘parallel orientation search’ feature, a feature that had been more or less discarded due to poor performance. As described in this post and this post, I have been able to obtain much better accuracy and precision from the ST Micro VL53L0X sensor arrays.

With this in mind, I decided to revisit my old ‘Rolling Turn’ algorithm, to see if I could get it’s performance up to the point where it could be used instead of ‘Spin Turn’ for the ‘parallel find’ maneuver. The Spin Turn maneuver involves rotating one set of wheels backwards and the other forwards to effect a ‘spin in place’ action. The Rolling Turn maneuver involves rotating both sets of wheels in the same direction, but at different speeds. My going-in assumption was that a rolling turn should allow more accurate control of the turn rate, as there should be less acceleration involved.

In preparation for this project, I went back to the web and read some more about PID tuning. This time I found this well-written and informative post by ‘marco_c’ – Thanks marco!

I had dropped the original ‘RollingTurn’ function from the code some time ago, so I started this experiment by copying SpinTurn() and adapting it to move both sets of wheels in the same direction (forward in this case) instead of in opposite directions. The conversion from SpinTurn and RollingTurn took surprisingly little effort, as everything but the motor direction code is pretty much the same.

I created a part-task program by creating a new ‘WallE3_RollingTurn_V1’ arduino project in VS2022 Community, and then copying my WallE3_WallTrackTuning_V4. The WallE3_WallTrackTuning_V4 project is also a part-task test vehicle, and as such had almost all the required parameter entry code already in setup().

After working my way through the usual number of mistakes, I finally got the project working smoothly. I copied captured telemetry output to Excel and used it’s excellent plotting facilities to investigate possible PID triplet combinations. After a day or so of work, I finally homed in on a triplet PID(1.1, 0.1, 0) as providing a pretty nice, smooth turning action for a 180º turn at 30º/sec. Then I tested the same configuration using both a 60º/sec and a 90º/sec commanded turn rate. As shown in the plots below, the robot did well in all three of these conditions:

180º turn at 30º/sec
180º turn at 60º/sec
180º turn at 90º/sec

Here’s a short video showing the 180º turn at 90º/sec configuration:

180º turn at 90º/sec

My test program as described above only handles forward motion CCW & CW turns. If I decide to put this feature back into WallE3’s main codebase, I’ll have to expand it a bit to handle the backward motion case as well.

05 December 2022 Update:

I modified the RollingTurn() function to accommodate both forward and backward motion, and both CW & CCW turns, so four cases.

Also, as I was doing this, it occurred to me that I might want to revisit my ‘SpinTurn()’ function in light of my better understanding of PID tuning. Maybe I can get SpinTurn to operate a little more smoothly.

Stay tuned,

Frank

Wall Parallel Find PID Tuning, Part II

2 Replies

Posted 01 December 2022

I thought I had the ‘parallel find’ problem solved about 18 months ago, back in April of 2021, but it seems this is a problem that refuses to die (well, up until now, anyway). This post describes the ‘new, new!’ solution, based on much improved distance measurement performance from the VL53L0X time-of-flight sensors and associated software. Basically, I discovered that I had been doing VL53L0X distance calibration all wrong, and a big part of the problem was my use of an integer instead of floating point data type to represent distance values. The VL53L0X sensor reports distances in integer millimeters, but when I converted the integer mm values to integer cm (without even rounding – but by truncation – yikes!) the resulting loss of precision caused significant problems with the parallel find algorithm.

So, after converting all my VL53L0X distance variables from integer to float (which turned out to be pretty easy as I had practiced good modularity and low coupling – yay!), I started over again with a two stage strategy. The first part addressed the calibration problem by redoing a series of tests to acquire compensation curves for both the right and left-side sensor arrays which were then programmed into the Teensy 3.5 processor that handles the VL53L0X arrays. The second part was to revisit the ‘parallel find’ algorithm. This post describes the result of the second part – revisiting the ‘parallel find’ algorithm.

The parallel find algorithm implements a two-stage search for the parallel condition, defined as the robot (or, more accurately, the relevant VL53L0X array) orientation that produces identical distance readings from the front and rear sensors of the relevant (i.e. left or right) array. The first ‘coarse’ stage searches for the change in sign of the steering value, and the second ‘fine tune’ stage searches for a steering value < 0.01, meaning that the front and rear distance values are nearly identical.

Here is the code for the parallel find subroutine:

And here is a short video and the telemetry output for a successful parallel find run

So it appears that my original parallel find algorithm now performs much better, due mainly to the more accurate distance reporting obtained by changing from integer to float data type, and better distance compensation curves for each of the six sensors in the two VL53L0X arrays. Now I need to back-port this improved code into my main robot navigation code, probably by way of the Wall Track part-task program described in my earlier ‘More Wall Track PID Tuning‘ post.

29 December 2022 Update:

After going through some additional ‘part-task’ tune-up exercises, I started the process of integrating all the changes back into my Wall Tracking PID Tuning code. After the usual number of screw-ups I got the left-side offset capture feature working up to the point of turning back parallel to the wall. The code at this point just used ‘SpinTurn()’ to turn 30º in the opposite direction as the first turn away from the wall, with a note that this was done because ‘Parallel Find is fatally flawed’. Well, since I had just gotten through testing this feature I thought I could just drop it in. When I did, the robot promptly turned in the wrong direction and started spinning – yikes! Back to the ‘Parallel Find’ drawing board!

After making some code changes to make the part-task code a bit easier to use, I got everything working again, with basically the same PID values (100,0,0) as before. Here’s the output and a short video from a run:

30 December 2022 Update:

Not so fast, pardner! Well, it seems I was a bit premature about completing this effort – After a few more trials I realized this wasn’t working anywhere near as well as I thought – oops!

So, after lots more trials aimlessly wandering around PID space, I came up with new values that *seem* to work (for the left side case, at least). I also discovered that I really don’t need a two-stage process (‘coarse’ tune followed by ‘fine’ tune) to get a decent result, as long as the robot turns slowly enough to allow the distance measurement changes to keep up. Here’s are a couple of runs (telemetry output and short video) for the new setup.

05 January 2023 Update:

Well, even the above ‘slow as she goes’ idea doesn’t work all the time or that well. I wound up trying to instrument what is going on by doing the following

  • Turning the robot in 10⁰ steps, followed by a 1500 mSec pause to let the measurements catch up
  • At 100mSec intervals during the 1500mSec pause, computing and displaying the 5-pt running average of the left front and rear distances, along with the running average computation of the steering value.

When I did this, I got the following plot

1500mSec plot of the steering value computed from a 5-pt running average of the left front and left rear distances. Each line is a different 10deg angle orientation, with the lowest (yellow) line representing a parallel or near-parallel orientation

As can be seen from the above plots, there is significant variation over the 1500mSec interval, even though the robot is stationary. In particular, the last plot shows that at the start of the 1500mSec stationary period, the steering value goes from 0.15 (i.e. not parallel at all) to near zero (i.e. very parallel), even though the robot isn’t moving at all.

I have no idea why this is happening, but it sure screws up any thoughts of rapidly finding a parallel orientation, and even sheds significant doubt on the entire idea of using multiple VL53L0X sensors for wall tracking – UGH!!

This time I tried a ‘parallel find’ run using 5⁰ SpinTurn() steps, with no averaging. Here’s the data, and a short video showing the result

This actually looks pretty good, and the unaveraged distance sensor results, although noisy, behaved reasonably well.

07 January 2023 Update:

As I was drifting off to sleep After yesterday’s ‘successful’ trial runs, I was happy visualizing the robot using the ‘SpinTurn() facility to gradually turn to the (mostly) parallel position (I’m a visual person, so I often turn programming problems into visual ones), it suddenly struck me that I was taking the long way around the barn; here I was sneaking up on the parallel orientation by small SpinTurn increments waiting for the steering value to change signs, when actually all I had to do was use my ‘GetWallOrientDeg(float steerval)’. This function takes the current steering value as the sole parameter and returns the current orientation angle with respect to the nearest wall; with this information I could use SpinTurn() to rotate to the parallel orientation in one shot – cool!

So, I recoded my test program to do just that, with the ‘enhancement’ (I hope) of taking a second or so at the start to acquire a 10-point average of the steering value, so as to, hopefully, reduce errors due to noisy distance sensor outputs. Here’s the output and a short video showing the result:

This seemed to work very well, and is a much simpler algorithm than trying to use an ‘on the fly’ steering value and a PID machine. I believe with this result I can get back to the problem I was originally trying to solve – that of reasonable wall following.

02 February 2023 Update:

Referring to my current quest to incorporate the results from all my previous ‘part-task’ programs into a ‘WallE3_Complete_V1’, I have been going through each ‘part-task’ program to verify results, and then incorporating the results (usually in the form of a single function) into the ‘complete’ program. When I got to WallE3_ParallelFind_V1, I discovered that, although I had made quite a bit of progress, including drastically simplifying the ‘parallel find’ algorithm, it still didn’t work very well.

After some additional work, I now think that ‘parallel find’ is ready for inclusion in the complete program. Here’s the functional code from WallE3_ParallelFind_V1:

The above code will replace the code in ‘RotateToParallelOrientation()’. See my post on consolidating everything into a ‘Complete’ program for more details

Stay tuned,

Frank

Wall-E3 Right Wall Following Trial

Leave a reply

Posted 23 March 2022,

Earlier this month I was able to demonstrate a multi-lap left-side wall tracking run by Wall-E3 in my office ‘sandbox’. This post describes my efforts to extend this capability to right-side wall tracking.

Since I already had the left-side wall tracking algorithm “in the can”, I thought it would be a piece of cake to extend this capability to right-side tracking. Little did I know that this would turn into yet another adventure in Wonderland – but at least when I finally made it back out of the rabbit-hole, the result was a distinct improvement over the left-side algorithm I started with. Here’s the left-side code:

The above code works, in the sense that it allows Wall-E3 to successfully track the left-side wall of my ‘sandbox’. However, as I worked on porting the left-side tracking code to the right side, I kept thinking – this is awful code – surely there is a better way?

After letting this problem percolate for few days, I decided to see if I could approach the problem a little more logically. I realized there were two major conditions associated with the problem – namely is the robot’s initial position inside or outside the desired offset distance K? In addition, the robot can start out parallel to the wall, or pointed toward or away from the wall. Ignoring the ‘started out parallel’ degenerate case, this reminded me of a 3-parameter Karnaugh map configuration, so I started sketching it out in my notebook, and then later in a Word document, as shown below:

As shown above, I broke the 3-parameter into two 2-parameter Karnaugh maps, and the output is denoted by αT. After a few minutes it became obvious that the formula for αT is pretty simple – its either αR – αA1 or αR – αA2 depending on whether the robot starts out outside or inside the desired offset distance. In code, this boils down to one line, as shown at the bottom of the Karnaugh map above, using the C++ ‘?’ trinary operator, and choosing CW vs CCW is easy too, as a negative result implies CCW, and a positive one implies CW. The actual code block is shown below:

Here’s a short video of Wall-E3 navigating the office ‘sandbox’ while tracking the right-side wall.

So, it looks like Wall-E3 now has tracking ability for both left-side and right-side walls, although I still have to clean things up and port the simpler right-side code into TrackLeftWallOffset().

25 March 2022 Update:

Well, that was easy! I just got through porting the new right-side wall tracking algorithm over to TrackLeftWallOffset(), and right out of the box was able to demonstrate successful left-wall tracking in my office ‘sandbox’.

At this point I believe I’m going to consider the ‘WallE3_WallTrack_V3’ project ‘finished’ (in the sense that most, if not all, my wall tracking goals have been met with this version), and move on to V4, thereby limiting the possible damage from my next inevitable descent through the rabbit hole into wonderland.

Stay tuned,

Frank

Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part VIII

Leave a reply

Posted 19 February 2022,

At this point in the evolution of Wall-E3, all the hardware seems to be working, so it’s time to get serious about wall tracking. Last fall I made another run at wall tracking with Wall-E2, and wound up with an algorithm that would first capture the desired wall offset, and then track it ‘forever’. This worked great, but the approach is at odds with the general processing architecture. The current tracking architecture is set up as a loop, where all pertinent parameters are updated every loop period, and the appropriate action is taken. In the case of wall tracking, the ‘action’ was one left/right motor speed update. This allows rapid recognition of, and adaptation to, various ‘error’ conditions, like being stuck or about to run into something. The algorithm developed last fall does none of this, so it can’t react properly (or at all, for that matter) to things like an upcoming wall.

I’m starting to think I can use a hybrid approach – use the current capture/tracking algorithm pretty much as it stands from last fall, but have it check for ‘error’ conditions each time through its own internal loop. If any unusual conditions are detected, then force an exit from the tracking routine and another pass through the main ‘traffic director’ function ‘GetOpMode()’. The updated mode assignment will then percolate back down through loop() and cause the appropriate handling function to be called.

22 February 2022 Update:

As a start, I ported the ‘TrackLeft/RightWallOffset() functions to Wall-E3 and, after the normal number of screwups and mistakes, I got the left side tracking algorithm working, as shown in the Excel plot and short movie clip below:

Here’s the complete code for ‘TrackLeftWallOffset()’:

The above algorithm works great, but it runs in an infinite loop once it has captured the wall offset. Based on my above comments about a hybrid approach, I could add tests for stuck and/or front or back obstacles to this loop (instead of the current ‘while(true)’). This would cause TrackLeftWallOffset() to exit, and the GetOpMode() function could assign the appropriate mode, which would then cause the proper function to execute.

Or, I could eliminate GetOpMode() entirely and put it’s logic in ‘loop()’? Actually, looking at the loop() function in FourWD_WallE2_V12.ino, my last iteration with the Arduino Mega2560, I see that GetOpMode() is called at the start of loop(), and then the OpMode switch statement comes pretty much immediately afterwards. Here’s the code:

The MODE_CHARGING and MODE_HOMING cases are self-contained, so no changes would be needed for them. The MODE_WALLFOLLOW case is sub-divided into TRACKING_LEFT and TRACKING_RIGHT cases. If all the inline code in TRACKING_LEFT was replaced with TrackLeftWallOffset() and that of TRACKING_RIGHT with TrackRightWallOffset(), with these two functions augmented by the current if (bIsStuck) , if(bObstacleAhead) and if(bObstacleBehind) guard code (pretty much as it now stands), then that should work. I think I’ll give that whirl and see what happens.

To start the process, I created yet another project – WallE3_WallTrack_V3 (to preserve the currently ‘working OK on left side’ status of WallE3_WallTrack_V2) and try porting the GetOpMode() and loop() code from FourWD_WallE2_V12.

02 March 2022 Update:

I now have a ‘loop() only’ version of WallE3 running that properly tracks the left side. Everything is basically the same as before, except the loop() function, shown below:

The ‘IR HOMING’ and ‘CHARGING’ blocks are essentially unchanged, and the ‘WALL TRACKING’ block is much simpler. All ‘anomaly’ (robot stuck either forward or backward, robot approaching an obstacle ahead or behind, dead battery, etc are all handled internally to the two ‘TrackLeft/RightWallOffset()’ functions. Here’s the (potentially infinite) ‘while()’ loop:

As the code above shows, the while loop will continue to execute as long as the ‘errcode’ value is ‘NO_ANOMALIES’. Internally the ‘CheckForErrorCondx()’ function surveys the inputs from all sensors and attempts to detect any anomalous behavior. Any return value except NO_ANOMALIES causes the while() loop to exit. Each potential anomaly condition has its own handling function, which exits back to ‘loop()’ and the process starts all over again. I believe this is a much cleaner approach than I had before with the ‘GetOpMode()’ function.

I also took the opportunity at this point to fix a long-standing problem with the code. The front-facing LIDAR unit returns distances in Cm, while all seven VL53L0X time-of-flight distance sensors report in mm. Not only did this torture me mentally (let’s see – is it Cm or mm here?), but it caused the new ‘CalcRearVariance()’ function to crater, because the 10x larger numbers, when squared, caused the ‘uint16_t’ type to overrun and produce crazy variance numbers. So, I changed the GetRequestedVL53L0XValues() function to convert mm to Cm, changed all the variable names from xxxxMM to xxxxCm and carefully combed through the entire codebase, correcting the inevitable wash of ’10x’ errors. In the end though, I made the codebase much more consistent and understandable (I hope).

Stay Tuned,

Frank