Tag Archives: VL53L0X

Left Side Wall Tracking Success With VL53L0X Array, Part II

Posted 10 October 2020

After the left-wall tracking success described previously in this post, I made some more adjustments and also set up a ‘ tracking sandbox’ in my lab to test Wall-E2’s ability to detect & respond to upcoming obstacles. Here’s a short video showing Wall-E2 in action

Tracking run demonstrating obstacle avoidance maneuvers

Here’s the raw output from the run:

And here is an Excel plot of just the movement sections of the above, highlighting the avoidance maneuvers.

left-side wall distances are shown in mm, while the front distance is shown in cm. Note 1-2 sec gaps during turns

Comparing the Excel plot to to the video, the front distance plot shows a monotonically decreasing value and then a large jump after each obstacle avoidance turn. It appears that the robot acquires and tracks the 30cm offset target successfully on the first wall, but doesn’t do as well on the second one. It was much more successful on the third wall. The plot for the last wall is only about 2 seconds long.

All in all, this looks like a pretty successful run for Wall-E2. It tracked three different walls (the fourth wall was too short to track) and successfully avoided obstacles three times – woo hoo!

12 October 2020 Update:

On the above ‘sandbox’ run, I noticed that at the end of the third leg at about 14 seconds into the run, the ‘spin turn’ at the white foam core wall wasn’t a ‘step turn’, but a ‘backup and turn’ triggered by the front distance going below the front obstacle limit of 20 cm, rather than the tracking obstacle clearance limit of 30 cm. Here are two output lines that illustrate the difference

and

In the video, these events are at about 7 & 14 seconds respectively. From this I came to the conclusion that at least the front distance wasn’t getting updated enough to keep the robot from getting too close to the obstacle before it realized there was a problem. At the time, the update rate for the system was set at 5Hz or 200 mSec. If the robot is travelling at 50 cm/sec, it means that it will travel 10 cm between distance updates – ouch!

So, I changed the timer interrupt timeout value for a 10Hz rate, and ran the ‘sandbox’ run again. This time when I looked at the output I could see that each leg terminated with something like

and it was clear that the updates were happening about every 100 mSec. Here’s the output:

and a short video:

And an Excel plot showing the left wall and forward distances progressing through the run.

Note that the front distance is shown in cm, while the left wall distances are shown in mm

At this point, I’m pretty happy with Wall-E2’s new-found wall tracking superpowers, at least for the left wall case. Now I need to port the V7 left-side-only code back into the main program and also port it to the right wall case.

Stay tuned!

Frank

Left Side Wall Tracking Success With VL53L0X Array

Posted 05 October 2020

This post describes the successful left-side wall-tracking performance of my re-motored, re-wheeled, and re-sensored robot. Back in January of this year I was able to demonstrate reasonable wall tracking performance with my two-wheel robot using the old HC-SR04 ‘Ping’ sensors. However, I still wasn’t able to consistently track and maintain a desired wall offset, the main goal in this project stage

Since January, I have made the following changes to my larger four-wheel robot:

With all the changes, I had kind of lost track of the ultimate goal, which is to have the robot follow the nearest wall at a specified offset distance. All of the above updates were intended, in one way or another, to facilitate that goal, but I hadn’t yet got the robot to actually perform to expectations.

To help clear away some of the fog, I created a new version of the operating software that was pared down to just what was required to track the left wall, and nothing else. The idea was to work out all the bugs for offset capture and subsequent wall tracking with just the minimum required software, and then incorporate the modified code back into the mainstream software.

At first I was working with a 4-stage process;

  • find the parallel heading to the selected wall
  • drive at an angle toward the desired offset distance
  • when the offset distance is obtained, turn parallel to the wall again
  • track the wall at the desired offset

However, I found that the when the robot started off outside the desired wall offset, the second ‘turn to parallel’ operation took up too much space, both in terms of wall offset distance, and distance along the wall. By the time the second ‘find parallel’ operation was completed, the robot was usually much too close to the wall for effective offset tracking, meaning the entire 4-step process would have to be repeated. So, I eliminated step 3 in the process (the second ‘turn to parallel’ operation) entirely, and modified the wall tracking algorithm to capture the desired wall offset and track it. Instead of using the distance sensor measurements directly, I generate a ‘steering value’ proportional to the difference between the front and rear sensor measurements, and a target ‘steering value’ proportional to the difference between the desired offset and the center sensor measurement and use a PID controller to match the measured steering value to the target steering value. The effect of this is that the robot will track toward the offset at an angle, and then turn parallel to the wall and continue to track, as shown in the video below:

Left-side offset capture and track demonstration

Here’s an Excel plot showing the wall offset distance versus time for the above demonstration run.

As can be seen in the above plot, the robot starts off at about 45 cm from the wall, tracks inward to capture the desired offset, and then continues to track the desired offset even when it goes around the 45-degree bend. The code that accomplished this is posted below:

Stay Tuned!

Frank

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays Part V

I have been running some wall-tracking tests with Wall-E2 and the new VL53L0X sensor array arrangement, but have been having poor results, especially with offset capture. After a bunch of test runs, I started to think that the distances aren’t updating fast enough to keep up with the robot’s forward speed, so it runs into the wall before it knows that it has gotten too close

Looking at the Teensy 3.5 I2C Slave code that manages the sensor array, I see the following loop() code

And I get the following output:

Looking at the timestamps, it appears that a measurement cycle takes about 200 mSec, taking into account the added 100 mSec delay from the delay(100); statement. This is consistent with the default 30 mSec measurement time for a single VL53L0X, but unfortunately this is much greater than the default 100 mSec PID controller update rate.

The VL53L0X can make measurements faster, but at the cost of lower accuracy. In my case, the increased accuracy from a 30 mSec measurement time is useless if it isn’t fast enough to keep up with the robot. Searching around the net, I found this post on the Pololu support forum, dealing with just this problem. So, I modified my Teensy 3.5 I2C Slave program to use ‘continuous measurements and the shorter (20 mSec vs 30 mSec) timing budget, as follows:

with the following results:

From the above it is apparent that the new loop time is about 19 mSec for all six sensors. This is very interesting, as it implies that in ‘continuous’ mode, all six sensors run all the time, and all the readContinuousMillimeters() function does is pull the latest measurement out of a buffer.

As a quick test, I rigged up a ‘fan blade’ (piece of paper taped to a old robot wheel on a motor) as shown, and then ran the program again with the motor spinning the ‘blade’ in front of the left-side sensor array (at about 100 RPM, I think). The plot shows that the sensor response is certainly fast enough to ‘see’ the rise and fall times on the ‘fan blade’.

03 October 2020 Update

With the above results in mind, I decided to try speeding up the ‘fan blade’ setup to see if I could find out how fast the VL53L0X sensor could go. I thought I should be able to use the shaft encoder setup on the back of the motor to acquire an accurate RPM reading and convert that into ‘milliseconds/blade’ to tell how short of an interval the VL53L0X could detect. As things often happen, determining motor RPM from encoder signals turned out to be a LOT harder than I thought. After a loooonnnng side-trip into geared-motor hell, I wound up more or less disregarding the encoder feature and modified the Teensy 3.5 ‘loop()’ code to produce a direct tachometer reading, as follows:

This allowed me to directly monitor ‘effective’ RPM & obstruction frequency. So I set up the experiment using a ‘four blade fan’ as shown below, and monitored the obstruction detector output with my Hanmatek DSO

DSO Output from VL53L0X Obstruction Detection loop() code

As can be seen from the DSO screenshot, the obstruction detection pulse frequency is about 26Hz, with a period of a little over 38 mSec. So it is clear that the VL53L0X running in continuous mode with a timing budget of 20 mSec can easily produce readings every 30 mSec or so.

04 October Update:

The next step was to see if the ‘VL53L0X fast/continuous’ code running on the Teensy VL53L0X sensor array manager would allow the main robot MCU to fetch distance readings faster. To do this, I uncommented the #define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop line in my program to eliminate all code except a short loop displaying distances. Then I took measurements with my 4-blade ‘fan’ running in front of the left-front sensor. I ran the motor voltage up to the point where the Teensy’s blade sensor output was showing about a 20Hz blade rate, and got the following output from the main MCU ‘DISTANCES_ONLY’ loop.

From the above, it is clear that the main MCU code can ‘see’ sensor output changes occurring at 20 Hz (50 mSec period). This should be fast enough to keep up with the physical movement of the robot during offset capture and wall-tracking activities.

In theory, I won’t have to do anything to the main MCU code to enjoy the faster response

Stay Tuned!

Frank

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays Part IV

Posted 18 July 2020

In my continuing effort to update Wall-E2’s superpowers, I have been trying to replace the HC-SR04 ‘ping’ sensors with ST Microelectronics VL53L0X Time-of-Flight (ToF)  sensors, as implemented by the popular GY530 modules available on eBay.

First, I got a 3-element array working and demonstrated effective parallel-heading determination and wall tracking, as described in this post.

Next, I added a second 3-element array on the other side of the robot, but I have been running into trouble getting both arrays to work properly at the same time.  Somehow there seems to be some interaction between the two arrays that I can’t seem to nail down.

  • I have determined that all six elements respond properly when operated individually or as a member of a 3-element array
  • Adding a 4th element to the array causes one or more of the first three elements to respond with an out-of-range measurement
  • Adding 2.2K pullups to the I2C bus makes the problem worse, not better.  After some investigation, I discovered that the GY530 module already has 10K pullups included, so three modules on the bus would reduce the pullups to 3.3K, and four would already reduce the value to 2.5K.  Adding a 2.2K in parallel with 3.3 or 2.5K would drive the value down to around 1.2-1.5K.  However, that did lead me to my next idea – using separate I2C busses for the left and right 3-element arrays.
  • Moved the left-hand 3-element array from the Wire1 I2C bus to the Wire2 I2C bus.  Now the 10K pullups shouldn’t be an issue, as I had already demonstrated proper operation of a 3-element array on the Wire1 I2C bus.  Unfortunately, this exhibited similar problems; When running all six elements, all three left-side elements measure properly, but only one right-side element produces reasonable values – the other two give nonsense readings.

Here’s a photo of the top deck of my autonomous wall-following robot, with the two 3-element arrays installed on the Wire1 and Wire2 I2C busses of a Teensy 3.5.

two 3-element VL53L0X arrays installed on a Teensy 3.5 Wire1 & Wire2 busses

And here is the schematic for the split-bus configuration:

A typical output sequence follows:  The first column is milliseconds since program start, and the following 6 columns are the front, center, and rear sensor measurements for the right & left arrays, respectively.

In the above, the data from the first two sensors on the right side is invalid, but all the rest show ‘real’ values.

If the left-side array is disconnected (unplugged) from the Wire2 bus and the program modified to not initialize/measure the left-side array, then the right-side array reads normally, as shown below

If the right-side array is disconnected (unplugged) from the Wire1 bus and the program modified to not initialize/measure the right-side array, then the left-side array reads normally, as shown below

Here’s the code being used to drive just the left-side VL53L0X array (right-side array code commented out and the right-side array physically disconnected from the Teensy 3.5):

And the same program, with the left-side array commented out

So, it appears that there is some sort of interaction between the Wire1 & Wire2 I2C busses on the Teensy 3.5.

22 July 2020 Update:

Based on some feedback from the Teensy forum, I added some code to my program to verify that each VL53L0X sensor I2C address had been set properly in the setup code.  When I did this, I got results that were more than a little mystifying; Initialization of the 3 sensors on the Wire1 bus all reported success, but the I2C scanning code reported a different story, as shown below:

The three sensors on the Wire1 bus were supposed to wind up at 2A, 2B & 2C, but the scanner showed them at 29 and 2C, with one of them missing entirely – wow!

So, I decided to go back to basics.  I modified my original triple VL53L0X demo program to include a I2C bus scan to verify the actual addresses of the sensors, as follows:

And got the following output:

As shown above, the VL53L0X sensors got programmed correctly, and appear to operate correctly as well.

Then I created a new program identical to the above, except for using the Wire2 bus instead of the Wire 1 bus, using different I2C addresses and different XSHUT pin assignments for programming the sensors.

When I ran this program, I got the following output:

Then I modified my original hex-sensor program to initialize one array at a time, with a I2C bus scan in between, as follows:

When I ran this program, I got the following output:

So it seems pretty clear that there is something going on with the Teensy 3.5 that doesn’t like it when I try to run both Wire1 & Wire2 buses at the same time.

As additional background data, the original impetus for splitting the six sensors between two I2C buses was my discovery that adding the 4th through 6th sensors on the Wire1 bus caused a similar problem to the one described here; clearly bad readings from the 1st & second sensors, while readings from later ones were fine.  I don’t know if these issues are related, but something is happening for sure.

23 July 2020 Update:

I’m frustrated at the lack of response from both the Teensy and ST Micro support forums on this issue.  The Teensy guys are trying to help, but nobody wants to look at the elephant in the room – the fact that all six VL53L0X units work fine when their respective I2C bus is the only one operating, but not when both buses are in operation.  The ST Micro guys just don’t answer at all.

I went back and modified my program to print out as many of the detailed measurement parameters as I could find for each sensor, in an effort to gain some understanding about what is happening, and got the following output:

This output style is much harder to read, but is also much more complete. Each line (distances, signal rate, SpadCount, and RangeStatus) has six entries – one for each of the six sensors.  The first three entries are sensors 1-3 on Teensy Wire1, and the remaining three are sensors 4-6 on Teensy Wire2.  As the data shows, sensors 1 & 2 always have bogus results, while sensors 3-6 have what appears to be valid data, although I’m not competent to say anything more than “the distance values for sensors 3-6 track reality, while the ones for sensors 1-2 do not”.

Then I modified the program yet again to just use sensors 1-3 on Teensy Wire1 I2C bus, without changing any hardware (leaving sensors 4-6 still attached to Teensy Wire2, but not initializing or addressing them in any way, and got the following output:

Now the parameters for sensors 1-3 all look real, and of course all the parameters for sensors 4-6 are zeroed out.

Then I modified the program to just initialize and access sensors 4-6 on Teensy Wire2

Now it is clear that the data for sensors 1-3 (Teensy Wire1) are all zeroes, and the data for sensors 4-6 (Teensy Wire2) are valid.  Again, this is with no hardware changes at all; all sensors are still powered and connected to their respective I2C buses.

29 July 2020 Update:

Still working the multiple VL53L0X issue.  After getting nowhere with the Teensy and ST Micro forums, I decided to try a different tack.  I decided to try controlling all six VL53L0X sensors using the single I2C bus on an Arduino Mega.  I reasoned that if I could get them all to play using a Mega, this would lend credence to my theory that something funny is going on with the Teensy 3.5 auxiliary I2C buses.

Unfortunately, I immediately ran into problems getting multiple sensors to work using the Arduino Mega.  At first I thought this was due to the fact that the Mega is a 5V controller and so I needed a level shifter setup on the I2C bus between the Mega and the VL53L0X sensors, but that didn’t change anything.  Then, after a more thorough look at the VL53L0X schematic and documentation I discovered that the real problem was that while the I2C bus lines have internal level shifters already implemented on the module, the XSHUT & GPIO lines do not.  This meant that I had been driving the XSHUT line of each of my attached sensors well over the do-not-exceed level — oops!

At this point I was starting to wonder if I had damaged the sensors’ XSHUT lines, thereby making any further diagnostic attempts with these sensors fruitless.  In addition, I was starting to wonder if I hadn’t also given myself problems by using ‘no-name’ modules – cheap, but maybe worth every penny?  I also had read some posts that indicated that the Adafruit VL53L0X driver library might have some problems, so maybe I had a trifecta going – cheap no-name modules, potentially damaged by my abuse of the XSHUT lines, being driven by a questionable library – yikes!

So, I started over; I acquired some Pololu VL53L0X modules, installed their Arduino driver library, and used their ‘Single’ code example to verify basic operation with a single VL53L0X sensor connected to an Arduino Mega controller.  Then I added in the multi-sensor initialization code, being careful to simply switch the XSHUT lines from output (for outputting a LOW signal) to input (for ‘outputting’ a HIGH signal by allowing the onboard 47K pullup to take over) for XSHUT line management.

With the above setup I have been able to demonstrate a working 3-sensor setup using an Arduino Mega controller.  When my remaining Pololu VL53L0X modules arrive later this week, I hope to show that I can run (at least) six Pololu VL53L0X sensors on a single I2C bus.  If I can do that, then I’ll be in a position to make some more waves on the Teensy forum (I hope).

By the way, one of the side-effects of this effort was a reply from John Kvam mentioning that ST Micro makes an Arduino compatible 3.3V 32-bit microcontroller called the ST32 (also known as the ‘blue pill’ for it’s PCB color).  This is pretty capable device, with the single drawback that it doesn’t come with an Arduino bootloader installed, meaning it can’t be directly programmed via its USB-C connector.  Instead, one has to use a FTDI device (like the CKDevices FTDI Pro or the Sparkfun FTDI breakout board.  However, there are plenty of “How To’s” out there describing how a bootloader can be loaded into flash memory, after which you can program it just like any other Arduino device – pretty cool!  Anyway, I ordered a couple of these boards to play with the next time I need something Arduino-ish but not as fast (or expensive) as a Teensy.

31 July 2020 Update:

Success!  I finally got more than three VL53L0X sensors working on the same I2C bus using an Arduino controller!  However, I’m embarrassed to say that in the process, I discovered a hidden broken ground wire in one of my two I2C bus daisy chains, and this may have been causing the symptoms I was seeing with the Teensy 2-bus configuration – don’t know yet.

In any case, after repairing the wire break I got a set of six VL53L0X sensors working, consisting of the three Pololu modules I just got in, plus three of the older GY530 modules I was using on earlier Teensy-based experiments.  After that, I was able to demonstrate proper operation of the two 3-sensor arrays from my Wall-E2 robot, as shown in the following photo and Excel plot.

Sensor arrays dismounted from Wall-E2 and connected to Arduino Mega I2C bus

01 August 2020 Update:

Well, it is officially time to eat crow.  After all the whooping and hollering I’ve done, it turns out the entire problem was a hidden ground wire break in I2C daisy-chain cable attached to the Teensy 3.5’s Wire2 I2C bus.  After repairing the break, I can now demonstrate operation of six VL53L0X sensors on two different I2C buses on the Teensy 3.5, as shown in photo and Excel plot below.

Six VL53L0X sensors on Teensy 3.5 Wire1 & Wire2 I2C buses. Note ground wire repair on left rear connector (top right in photo)

Now that this saga has been thankfully resolved, I can get back to the original project of integrating these two sensor arrays onto Wall-E2, my autonomous wall-following robot.

August 08, 2020 Update:

I believe I have finally completed the effort to integrate the VL53L0X sensor arrays onto Wall-E2, my autonomous wall-following robot.  Here’s the physical setup

Dual 3-element VL53L0X sensor arrays on top deck of Wall-E2.

Note that the USB cable to the Teensy 3.5 is temporary, just for testing.  To verify proper operation, I wrote a small program for the Mega 2560 main controller containing only the code  from the main FourWD_WallE2_V5.ino required to retrieve sensor values from the Teensy 3.5, and used this program to verify and debug the Teensy 3.5 program. As the two Excel plots below show, the main Mega 2560 controller can now retrieve distance data from all six VL53L0X sensors at once.

VL53L0X distances reported locally by the Teensy 3.5

VL53L0X distances as retrieved from the Teensy 3.5 by the Mega 2560

There are some very small differences in these two plots, which I attribute to the fact that the Teensy measurement timing and the Mega 2560 retrieval timing are asynchronous, so the Mega may be retrieving some new and some ‘old’ (in the sense that it might be 100 mSec older than the rest) measurement data.  This is insignificant operationally, and wouldn’t be evident unless this sort of simultaneous local/remote reporting was done.

A minor side note; I wound up using the GY530 ‘no name’ sensors rather than the Pololu ones because they were a) smaller, and b) already mounted on the two custom brackets I printed for them.  The Pololu sensors (along with a whole bunch of GY530’s that finally arrived from Ali Express) went into my ‘Sensors’ parts bin for the next project.  If anyone needs VL53L0X sensors, let me know! 😉

Stay tuned,

Frank

 

 

 

 

 

 

I2C Hangup bug cured! Miracle of Miracles! Film at 11!

Posted 06 July 2020

Miracle of miracles!  Arduino finally got off their collective asses and decided to do something about the well-known, well-documented, and long-ignored I2C hangup bug.  Thanks to Grey Christoforo of Oxford, England for submitting the pull request that started the ball rolling.  See this  github issue thread for all the gory details.  However, in a bizarre outcome, the implementation of the needed timeouts isn’t implemented by default! You have to modify your code to add a call to a new function, like the following:

Note that you have to explicitly add a timeout value (3000 in my example above) or the timeout feature will still not be enabled! The ‘true’ parameter tells the library to reset the I2C bus if a timeout is detected – surely something you will want to do.

I’m currently working on a ‘before/after’ post to demonstrate that the new timeout feature actually works with real hardware scenarios.  However, due to the intermittent nature of the I2C hangup bug, it takes a while (hours/days) to grind through enough iterations to excite the bug reliably, so it may be a while before I have a good demonstration

One last thing; at some point the examples in C:\Program Files (x86)\Arduino\hardware\arduino\avr\libraries\Wire\examples (on my Win 10 machine) will probably be updated/expanded to show how to properly implement the new timeout feature, but this has not happened yet AFAICT.

The rest of this post describes my attempt to verify that the new timeout feature does, in fact, work as advertised.  The idea is to construct a “before-and-after” demonstration, where the ‘before’ configuration reliably hangs up using the Wire library without the timeout enabled, and an ‘after’ configuration that is identical to the ‘before’ setup except with the timeout enabled.

Before Configuration:

I actually started with a ‘before-before’ configuration using the SBWire library, as I have been working with I2C projects and the SBWire library ever since I gave up on the Arduino Wire library two years ago.  This configuration is patterned after Wall-E2, my current autonomous wall-following robot, which uses an Adafruit RTC, an Adafruit FRAM, a DFRobots MPU6050 IMU, and six VL53L0X time-of-flight proximity sensors (the ToF sensors are managed by a slave Teensy over the I2C bus).  For this test, I arranged all the I2C components on a plug board and connected to them using an Arduino Mega 2560 (the same controller I have on Wall-E2), as shown in the following photo.

From left to right; two VL53L0X ToF modules, FRAM module, DS3231 RTC module, MPU6050 IMU module

The software is a cut down version of the robot software, and in this first test all it does is print out time/date from the RTC and the relative heading value from the IMU.  After almost 13 hours, it was still running fine, as shown below:

So now I have a ‘known good’ (with SBWire) hardware configuration.  The next step is to change the software back from SBWire to Wire without the timeout implemented.  This should fail – the IMU readout should hangup within a few hours as it did before I originally switched to SBWire.

July 08 2020 Update:

After laboriously changing back from SBWire to Wire, I got the configuration shown in the following photo to work properly using the new Wire library without the new timeout feature enabled.

From left to right: MPU6050 IMU, DS3231 RTC, Adafruit I2C FRAM, and 3e VL53L0X ToF proximity sensors, all on the Mega 2560’s I2C bus

I programmed the Mega to access everything but the FRAM 10 times/second, and print out the results on the serial monitor, and then let it run overnight.  When I got up this morning I expected to see that it had hung up after a few hours, but discovered that it was still running fine after eight hours – bummer!  at 10 meas/sec that is 480 min * 60 sec/min * 10 = 288,000 I2C measurement cycles * 5 I2C transactions per cycle = 1,440,000 I2C transactions.  I was bummed out because it will be impossible to verify whether or not the timeout feature actually works if I can’t get a configuration that reliably hangs up. When I came back a few hours later, I saw that the printout to the serial monitor had stopped at around 700 minutes, but this turned out to be the monitor hanging up – not the I2C bus – double bummer.

So, I modified the program to only report results every second instead of 10/second so I won’t run out of serial monitor again, and restarted the ‘before’ configuration.

10 July 2020 Update:

I added the Sunfounder 20 x 4 I2C LCD display to the setup so I could display the IMU heading and proximity sensor distances locally, as shown below

I2C Test setup with Sunfounder 20 x 4 I2C LCD added

After getting this setup running, I was trying to figure out how to definitively demonstrate I2C bus hangups without the Wire library timeout feature (the ‘before’ configuration) and then demonstrate continued operation with timeouts enabled (the ‘after’ configuration).  In an email conversation, Grey Christoforo pointed me to another poster who was doing the same thing, by using an external transistor to short one I2C line to ground under program control, thereby demonstrating that the timeout feature allowed continued operation.  This gave me the idea that manually shorting one of the I2C lines to ground should do the same thing, and would allow me to demonstrate the ‘before’ and ‘after’ configurations.

The following code snippet shows the code necessary to enable the Wire library timeout feature

Although not entirely necessary, this is how I instrumented my code to capture timeout events and display them on my serial monitor

All my other hardware setup code has been removed for clarity.  Notice though, that I tried a number of different timeout values, starting from the default value of 25000 (25 mSec) down to 2000, and then back up to 3000.  At least in my particular configuration, the 1000 value was too small – it caused a timeout flag to be generated on every pass through the loop.  This was an unexpected result, as the SBWire library uses a 100 uSec (i.e. a timeout value of 100) for it’s default timeout value, and this setting has always worked fine in all my I2C projects.

In any case, here’s a short video that demonstrates that the Wire library can now recover from an I2C bus traffic interruption via the use of the new timeout feature.

 

Stay tuned!

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays Part II

Posted 16 June 2020

In my previous post on this subject, I described my efforts to replace the ultrasonic ‘ping’ distance sensors with modules built around the ST Microelectronics VL53L0X ‘Time of Flight’ LIDAR distance sensor to improve Wall_E2’s (my autonomous wall-following robot) ability to track a nearby wall at a specified standoff distance.

At the conclusion of my last post, I had determined that a three-element linear array of VL53L0X controlled by a Teensy 3.5 was effective in achieving and tracking parallel orientation to a nearby wall. This, should allow the robot to initially orient itself parallel to the target wall and then capture and maintain the desired offset distance.

This post describes follow-on efforts to verify that the Arduino Mega 2560 robot controller can acquire distance and steering information from the Teensy 3.5 sensor controller via its I2C bus.  Currently the robot system communicates with four devices via I2C – a DF Robots MPU6050 6DOF IMU, an Adafruit DS3231 RTC, an Adafruit FRAM, and the Teensy 3.2 controller for the IR Homing module for autonomous charging.  The idea here is to use two three-element linear arrays of VL53L0X modules controlled by a Teensy 3.5 on its secondary I2C bus, controlled by the Mega system controller on the main I2C bus.  The Mega would see the Teensy 3.5 as a just a fifth slave device on its I2C bus, and the Teensy 3.5 would handle all the interactions with the VL53L0X sensors.  As an added benefit, the Teensy 3.5 can calculate the steering value for tracking, as needed.

The first step in this process was to verify that the Mega could communicate with the Teensy 3.5 over the main I2C bus, while the Teensy 3.5 communicated with the sensor array(s) via its secondary I2C bus.  To do this I created two programs – an Arduino Mega program to simulate the main robot controller, and a Teensy 3.5 program to act as the sensor controller (the Teensy program will eventually be the final sensor controller program).

Here’s the Mega 2560 simulator program:

And the Teensy 3.5 program

Here’s the hardware layout for the first test:

Arduino Mega system controller in the foreground, followed by the Teensy 3.5 sensor array controller in the middle, followed by a single 3-element sensor array in the background

The next step was to mount everything  on the  robot’s second deck, and verify that the Teensy 3.5 sensor array controller can talk to the robot’s Mega controller via the robot’s I2C bus.   Here’s the hardware layout:

Right-side three sensor array mounted on robot second deck. Teensy 3.5 sensor array controller shown at middle foreground.

After mounting the array and array controller on the robot’s second deck, I connected the Teensy to robot +5V, GND and the I2C bus, and loaded the VL53L0X_Master.ino sketch into the robot’s Mega system controller.  With this setup I was able to demonstrate much improved parallel orientation detection.  The setup is much more precise and straightforward than the previous algorithm using the ‘ping’ sensors.  Instead of having to make several turns toward and away from the near wall looking for a distance inflection point, I can now determine immediately from the sign of the steering value which way to turn, and can detect the parallel condition when the steering value changes sign.

I think I may even be able to use this new ‘super-power’ to simplify the initial ‘offset capture’ phase, as well as the offset tracking phase.  In earlier work I demonstrated that I can use a PID engine to drive a stepper motor to keep the sensor array parallel to a moving target ‘wall’, so I’m confident I can use the same technique to drive the robot’s motors to maintain a parallel condition with respect to a nearby wall, by driving the steering value toward zero.  In addition, I should be able to set the PID ‘setpoint’ to a non-zero value and cause the robot to assume a stable oblique orientation with respect to the wall, meaning I should be able to have the robot drive at a stable oblique angle toward or away from the wall to capture the desired offset.

19 June 2020 Update:

As a preliminary step to using a PID engine to drive the ‘RotateToParallelOrientation’ maneuver, I modified the robot code to report the front, center, and rear distances from the VL53L0X array, along with the calculated steering value computed as (F-R)/C.  Then I recorded the distance and steering value vs relative pointing angle for 20, 30, and 40 cm offsets from my target ‘wall’.

Here’s the physical setup for the experiment (only the setup for the 20 cm offset is shown – the others are identical except for the offset).

Experiment setup for 20 cm offset from target wall

Then I recorded the three distances and steering value for -40 to +40º relative pointing angles into an Excel workbook and created the plots shown below:

Array distances and steering value for 20 cm offset. Note steering value zero is very close to parallel orientation

Array distances and steering value for 30 cm offset. Note steering value zero is very close to parallel orientation

Array distances and steering value for 40 cm offset. Note steering value zero is very close to parallel orientation

The front, center, and rear distance and steering value plots all look very similar from 20-40 cm offset, as one would expect.  Here’s a combined distance plot of all three offset values.

Combined distance plots for all three offsets. Note that the ‘flyback’ lines are an artifact of the plotting arrangement

In the above chart, it is clear that the curves for each offset are very similar, so an algorithm that works at one offset should also work for all offsets between 20 and 40 cm.  The next chart shows the steering values for all three offsets on the same plot.

Steering values vs relative pointing angle for all three offset distances

As might be expected from the way in which the steering value is computed, i.e. (Front-Rear)/Center, the value range decreases significantly as the offset distance increases.  At 20 cm the range is from -0.35 to +0.25, but for 40 cm it is only -0.18 to +0.15.  So, whatever algorithm I come up with for achieving parallel orientation should be effective for the lowest range in order to be effective at all anticipated starting offsets.

To try out my new VL53L0X super-powers, I re-wrote the ‘RotateToParallelOrientation’ subroutine that attempts to rotate the robot so it is parallel to the nearest wall.  The modification uses the PID engine to drive the calculated steering value from the VL53L0X sensor array to zero.  After verifying that this works (quite well, actually!) I decided to modify it some more to see if I could also use the PID engine and the VL53L0X steering value to track the wall once parallel orientation was obtained.  So I set up a two-stage process; the parallel orientation stage uses a PID with Kp = 800 (Ki = Kd = 0), and the tracking stage uses the same PID but with Kp set to half the parallel search value.  This worked amazingly well – much better than anything I have been able to achieve to date.

The following short video is composed of two separate ‘takes’ the first one shows a ‘parallel orientation’ find operation with Kp = 800.  The second one shows the effect of combining the ‘parallel find’ operation with Kp = 800 with a short segment of wall tracking with Kp = 400.  As can be seen in the video, the tracking portion looks for all the world as if there were no corrections being made at all – it’s that smooth!  I actually had to look through the telemetry data to verify that the tracking PID was actually making corrections to keep the steering value near zero.  Here’s the video

and here’s the output from the two-step ‘find parallel and track’ portion of the video

From the telemetry it can be seen that the ‘find parallel’ stage results in the robot oriented parallel to the wall at about 360 mm or so.  Then in the ‘track’ stage, the ‘Steer’ value is held to within a few hundredths (0.01 to 0.07 right at the end), and the offset distance stays practically constant at 361 to 369 mm – wow!

The results of the above experiments show without a doubt that the three VL53L0X linear array is quite accurate parallel orientation and wall tracking operations – much better than anything I’ve been able to do with the ‘ping’ sensors.

The last step in this process has yet to be accomplished – showing that the setup can be used to capture a desired offset after the parallel find operation but before the wall tracking stage. Based on the work to date, I think this will be straightforward, but…..

21 June 2020 Update:

I wasn’t happy with the small range of values resulting from the above steering value computation, especially the way the value range decreased for larger offsets.  So, I went back to my original data in Excel and re-plotted the data using (Front-Rear)/100 instead of (Front-Rear)/Center.  This produced a much nicer set of curves, as shown in the following plot

Steering values vs relative pointing angle using (Front-Rear)/100 instead of (Front-Rear)/Center

Using the above curve, I modified the program to demonstrate basic wall offset capture, as shown in the following short video

The video demonstrates a three stage wall offset capture algorithm, with delays inserted between the stages for debugging purposes.  In the first stage, a PID engine is used with a very high Kp value to rotate the robot until the steering value from the VL53L0X array is near zero, denoting the parallel condition.  After a 5 second delay, the PID engine Kp value is reduced to about 1/4 the ‘parallel rotate’ value, and the PID setpoint is changed to maintain a steering value that produces an approximately 20º ‘cut’ toward the desired wall offset value.  In the final stage, the robot is rotated back to parallel, and the robot is stopped.   In the above demonstration, the robot started out oriented about 30-40º toward the wall, about 60-70 cm from the wall.  After the initial parallel rotation, the robot is located about 50 cm from the wall.  In the offset capture stage, the robot moves slowly until the center VL53L0X reports about 36 cm, and then the robot rotates to parallel again, and stops the motors.  At this point the robot is oriented parallel to the wall at an offset that is approximately 28 cm – not quite the desired 30 cm, but pretty close!

The next step will be to eliminate the inter-stage pauses, and instead of stopping after the third (rotate to parallel) stage, the PID engine will be used to track the resulting offset by driving the VL53L0X array steering value to zero.

23 June 2020 Update:

After nailing down the initial parallel-find, offset capture  and return-to-parallel steps, I had some difficulty getting the wall tracking portion to work well. Initially I was trying to track the wall offset using the measured distance after the return-to-parallel step, and this was blowing up pretty much regardless of the PID Kp setting.  After thinking about this for a while, I realized I had fallen back into the same trap I was trying to escape by going to an array of sensors rather than just one – when the robot rotates due to a tracking correction, a single distance measurement will always cause problems.

So, I went back to what I knew really worked – using all three sensor measurements to form a ‘steering value’ as follows:

To complete the setup, the PID setpoint is made equal to the measured center sensor distance at the conclusion of the ‘return-to-parallel’ step.  When the robot is parallel to the wall, Front = Rear = 0, so the Steering value is just the Center distance, as desired, and the PID output will drive the motors to maintain the offset setpoint.  When the robot rotates, the front and rear sensors develop a difference  which either adds to or subtracts from the center reading in a way that forms a reliable ‘steering value’ for the PID.

Here are a couple of short videos demonstrating all four steps; parallel-find, approach-and-capture, return-to-parallel, and wall-tracking.  In the first video, the PID is set for (5,0,0) and it tracks pretty nicely.  In the second video, I tried a PID set for (25,0,0) and this didn’t seem to work as well.

At this point I’m pretty satisfied with the way the 3-element VL53L0X ToF sensor array is working as a replacement for the single ultrasonic ‘ping’ sensor.  The robot now has the capability to capture and track a specific offset from a nearby wall – just what I started out to do lo these many months ago.

25 June 2020 Update:

Here are a couple of short videos in my ‘outdoor’ (AKA entry hallway) range. The first video shows the response using a PID of (25,0,0) while the second one shows the same thing but with a PID value of (5,0,0).

The following Excel plot shows the steering value (in this case, just Fdist – Rdist), the corresponding PID response, and the center sensor distance measurement

I Googled around a bit and found some information on PID tuning, including this blog post.  I tried the recommended heuristic method, and wound up with a PID tuning set of (10,0,1), resulting in the following Excel plot and video

 

Stay tuned!

Frank