Over-the-Air (OTA) Firmware Updates For Teensy 3.x & 4.x

Leave a reply

Posted 23 September 2021

For some time now I have been following developments on Paul Stoffregen’s Teensy forum with respect to OTA updates for Teensy products. As it turns out, doing OTA updates is less than straightforward due to the Teensy architecture (I’m waving my hands here).

However, in recent months, thanks to Teensy forum member joepasquariello, there is a new set of programs that seem to address the Teensy OTA issue in a much more comprehensive manner. See this thread (starting with post #85) on the Teensy forum for the details. After reading through the posts, and asking some more dumb questions (which joe was fortunately willing to answer), I decided to give this a try to see if I could achieve OTA updates for a Teensy controller, and maybe make it as convenient as OTA updates are now with Arduino products using the Pololu Wixel RF serial pipe.

The FlasherX package is actually three different files; FlasherX.ino is a demo program that #includes FlashTxx.h and FlashTxx.cpp. The FlasherX.ino program is loaded into the target Teensy device in the normal manner, just like any other Teensy program. However, once it is there, it adds the capability to update the Teensy’s firmware using the .HEX file (produced by the normal compile process) transmitted over any serial port. Of course, in order to make this magic happen, the updated Teensy program must also have this capability, so that means that any Teensy program destined for OTA updates must include the basic ‘FlasherX’ components.

In order to get started, I dl’d all three files and programmed a Teensy 3.5 with the FlasherX.ino program using my normal VS2019/Visual Micro setup. Then I created a new Arduino Project in VS2019 called ‘TeensyFlasher_V1’ and copied the entire contents of FlasherX.ino. I compiled it for a T3.5 target and used my normal Teensy bootload process to transfer it to the Teensy 3.5. Here is the serial port output after the upload:

Initial output from FlasherX.ino after upload to Teensy 3.5

Next I used Tera Term’s ‘file send’ capability to send the FlasherX.hex file to the Teensy using the wired USB connection as just another serial port. When I first launched Tera Term I got an error dialog saying it “Cannot open COM11. Access denied” – reminding me that I had to close the normal connection in order for Tera Term to operate properly. Once I had done this, I used the ‘File->New Connection menu item in Tera Term to create a new connection to COM11 (the Teensy 3.5 USB port), as shown:

New connection to Teensy port

Next I used ‘File->Send File… to select the .HEX file created by the compiler, and transfer its contents over to the Teensy using the COM11 serial port

After selecting the .HEX file and clicking ‘Open’, the HEX contents were sent over the serial connection as shown below:

The reason for using the HEX file created from compiling FlasherX.ino (as opposed to a random Teensy app) is because FlasherX.ino already contains all the pieces needed to do OTA updates. Although a bit recursive, using the FlasherX app demonstrates that a Teensy device can be updated via a serial connection repeatedly, as long as the newly updated firmware contains the pieces needed to support the next OTA update.

The next step was to figure out how to get VS2019/Visual Micro to do the update automatically, so that the process emulates what happens with an Arduino controller when F5 is pressed. VS2019 supports post-build scripts, but they aren’t used in the Visual Micro environment. Instead, as I learned from VM guru Tim Leek, VM supports post-build operations via a special file called ‘board.txt’ in the project directory. If this file is present, properly formatted lines are treated as ‘build property overrides’. For instance, this is the line in ‘board.txt’ that calls a C# console app to actually communicate with the target Teensy.

This command launches the console app, and provides it with the path to the project, the path to the sketch, and the currently selected port number. See this thread on the VM forum for all the gory details.

I ran into a problem when I tried to use the above command line with my C# console app, because the arg[] string array in the console app was interpreting spaces in the strings in the calling command differently than expected, so when I sent three strings to the command, the console app thought I was only sending two – somehow concatenating the com port number into the second string (the sketch path). After playing around some more, I found that the following treatment:

when passed as an argument to this small code snippet

produced the following output:

With the correct number of arguments, and the comport number on its own line – yay. I have no idea why this works and the other formulations don’t, and why the second argument has to be have it’s quotes escaped with ‘\’, but hey – it works.

As it turns out, this exercise was a bit academic anyway, because both the {ProjectDir} and {sketch_path} expand to identical results, so I only need one of them and the comport number to proceed. So, the ‘final’ (for now) call argument list looks like:

which produces:

27 September 2021 Update

In order to do OTA updates, I need to be able to connect to one of the Teensy’s hardware serial ports. Up til now I have been just using the USB serial port for convenience, so I thought I would try using a FTDI Pro USB/Serial converter module to connect to Serial1 on the teensy, as shown in the following photo:

In the above photo, the normal USB connection provides 5V power for the Teensy, and also allows ‘direct’ programming via the normal VS2019/VM route. The FTDI module connection shows up as COM3, a ‘regular’ USB serial port. With this setup I was able to upload a .HEX file to the Teensy using Tera Term, confirming that I can indeed use a hardware serial port for this purpose.

04 October Update:

Between the last update and now, I spent a lot of time trying to get the command-line post-build console app working, mostly to no avail. I kept getting wrapped around the axle trying to deal with three different serial connections; two to the teensy itself, and one from the command-line app to VS2019’s Console service. This got to the point where at one time the ‘Console’ output was going to my computer display, which made the mouse jump around wildly, occasionally launching or closing apps! Even trying to restart the PC was a trial-and-error process, because about half the time the screen focus would jump away from the target button before I could get it clicked!

So, I decided to go back to basics and use just the FTDI serial adaptor to run the Teensy. This required using the Vcc connection from the FTDI module to power the Teensy, which worked OK once I figured out how to switch the FTDI module from 3.3V out to 5V out.

FTDI USB-serial module now powering Teensy 3.5

With this setup I am able to upload programs to the Teensy using Tera Term, as before, but now I don’t have the other USB connection to confuse things. It does mean however, that now I MUST use Tera Term to upload sketches, as I no longer have the capability to use the Teensy bootloader from VS2019.

With this simpler setup, ‘Console’ statements from the command-line app should go only to the VS2019 debug session, and serial port operations from the command-line app should go only to Teensy’s ‘serial1’ hardware serial port.

05 October 2021 Update:

I expanded the above operation to restore the USB connection to the PC, and created a small Arduino program to (at first) just echo serial data from one serial port to the other. Here’s the program:

Once I had this going to my satisfaction, I started adding pieces from my ‘TeensyFlashDemoV2’ project until I had a complete working program, as shown below:

Here’s the Arduino code:

and here is the companion C# command-line app:

And here is the command-line output from my last run:

In the above printout, most of the HEX lines were removed just to keep the line count down, but all 2977 lines were successfully sent to the Teensy. The lines

were emitted from the Teensy at the conclusion of the HEX file upload. The line

was from the command-line app comparing the number of lines sent with the number of lines received. The numbers matched, so ‘2977’ was sent to the Teensy to complete the update.

At this point I have a working automated update system except the COM port number and HEX file location was passed to the C# program as a set of fixed arguments (as shown below), to disconnect the command-line app from the Arduino sketch, instead of dynamically providing the same parameters from VM build properties

Now that I have some confidence that the command-line app does what it is supposed to do, I can go back and make that connection again, hopefully with better results this time.

A problem with the Arduino-to-C# command-line app is that the ‘build properties’ exposed by Visual Micro aren’t always the same, so what works for one sketch may or may not work for another. Here’s the ‘build properties’ output from my expanded ‘TeensySerialBridgeV1.ino’ file:

I need two things from the build properties – the COM port number, and the path to the .HEX file. From the above list, it appears that I could use either of the following properties,

but neither is very convenient, as the COM port number has to be extracted from a ‘known’ string. On other projects, I have seen a build property like ‘serial.port.number = 11’ which would be much more convenient, but it seems to have disappeared. For the path to the .HEX file, I should be able to use

or

but it appears the ‘final_output_path’ is a bit more directly related to the .HEX file location. So, to make this work using the VM post-build facility, the required ‘board.txt’ file should look like:

When the above is placed in ‘board.txt’ and the Arduino sketch is run, I got the following output:

Which appears to be correct, and the Teensy was obviously updated (got the USB disconnect/connect tones, and the LED tell-tale stopped and then resumed blinking). It looks like I might actually have a working program now – woo hoo!

06 October 2021 Update:

One minor snag occurred with the build property used to pass the COM port number to the command-line program; I had been using {serial.port.caption} and it turns out that this parameter (along with many other serial.port parameters) isn’t always available – things change depending on the COM port chosen in the Arduino program and the actual device connected (or not connected) to the PC. After another round of posts in the Visual Micro forum, Tim Leek pointed out that the ‘serial.port’ parameter should always be available. I had missed this on this because it isn’t grouped in with the rest of the ‘serial.xxx.xxx’ parameters. With the benefit of hindsight, I can see why this is so; the ‘serial.port’ parameter is probably pulled from the VS2019 IDE, while the other ‘serial.port.xxx’ parameters are hardware-specific. Once I switched my ‘board.txt’ to use the ‘serial.port’ parameter vs ‘serial.port.caption’, I started getting much more consistent results. Here’s the ‘final’ (I hope) board.txt configuration:

One other small snag; my ‘TeensySerialBridgeV1’ program was originally intended to just pass serial data back and forth between two terminal programs (Tera Term on one side, the Teensy sketch on the other). As a result, there was a built-in assumption that both serial connections would always be available, as shown in the following code snippet:

The first ‘while()’ statement causes the program to hang if the physical Teensy USB connection is missing. This gave me some grief when I removed the Teensy USB cable to prove I could do the entire update cycle with only the Serial1 hardware serial device connected – oops!

The final Teensy program is shown below:

09 October 2021 Update: The final product

After encountering and solving a few more ‘gotchas’, I think I now have a reliable Teensy OTA mechanism using the VS2019/Visual Micro IDE and Joe Pasquariello’s wonderful ‘FlasherX’ code . The ‘trick’ is using Visual Micro’s ‘board.txt’ feature to add a post-build command that runs each time the Teensy sketch is compiled, and a small C# command-line program to trigger the FlasherX update code and feed it the correct Hex file.

I created a very simple Teensy project called ‘TeensyOTADemo’ that does only two things; it flashes the built-in LED, and launches the FlasherX update code when triggered by a ‘U’ (for Update) ascii code on Serial1 (pins 0 & 1 on a T3.5). Then I created a small C# command-line program to actually send the trigger character and then transmit the .HEX file created as part of the normal compile process.

Then I created a ‘board.txt’ file containing a post-build command that launches the C# command-line program and feeds it two arguments – one containing the build path, and the other containing the COM port number to be used to connect to the Teensy’s Serial1 port.

Here’s the Teensy sketch:

And the C# command-line program:

And last, the ‘board.txt’ file contents that launches the C# command-line program:


Here’s a short video showing the update process. The clip starts with the ascii ‘U’ trigger code arriving at the Teensy Serial1 port, and ends with the update finished and the Teensy rebooted back into the ‘blink’ code.

09 October 2021 Update: HC-05 Bluetooth link:

The next step is to replace the FTDI module with an HC-05 Bluetooth link. I had some HC-05 modules from previous work, and I had done a similar project with an Arduino some time back – so how hard could it be?

I hooked up a HC-05 to the Teensy as shown in the following photo, using a bench power supply to power both the HC-05 and the Teensy 3.5

HC-05 and Teensy both powered by lap power supply

Then I paired the HC-05 with my laptop; this took a bit of fumbling around, as the laptop thought it was still paired with an HC-05 even though it’s been over two years since I last used it, and I had since forgotten that the default password was ‘1234’. In any case, I eventually got it paired, as shown, with an automatically generated COM port on COM4.

Next I fired up Tera Term and created a connection to COM4, as shown:

And then tried sending some ascii text through to the Teensy. This appeared to work fine, but when I tried a program update via the HC-05 link I got an error that indicated the FlasherX code didn’t like something I was sending it.

The ‘abort – bad hex line’ output happens before I try to send it the .HEX file contents, so it clearly isn’t choking on that; maybe some extra characters getting sent instead of just the “U” character?

Looking at the ‘FlasherX’ code, it appears that the offending characters/line is supposed to be printed out with the lines

which implies whatever caused the problem is non-printable – maybe an extra CR/LF? So, I added some debug code to update_firmware() to print out each line as it is read into the ‘line’ character array, so maybe I can see the problem.

Hmm, more mysteries. I reconnected the regular USB connection, and uploaded the new version of the sketch. Then I disconnected the USB connector, and used Tera Term to send the update command (“U”). However, this time the OTA Update code didn’t choke, and AFAICT performed a completely successful update via BT and the HC-05 – yay! Of course, I now have no idea what caused the problem to start with, and whether or not my addition of a few lines of debugging code made the difference.

So, I undid the changes to the update code and tried again. And, as shown below, the OTA update worked perfectly – double Yay!!

The printed out lines from the .hex file are from the previous run.

So now to try an OTA update using my command-line program instead of Tera Term. I disconnected TT from COM4, and then, after selecting COM4 in the VS2019 IDE, pressed F7 to start the process. This worked as well, but it appeared the transfer rate was considerably slower – no idea why.

Here’s a short video showing the OTA update using the command-line program and BT OTA.

Teensy OTA update using command-line program and HC-05 BT link

Just comparing the overall times from the two videos above the one using the command-line program runs approximately twice as long. It’s hard to believe it is only the use of the command-line program vs Tera Term that is causing the delay – but it is possible that TT was simply running at a higher baud rate – more study required.

In summary, however, slow rate or no, it is clear that we now can implement completely automated, seamless OTA update processes using VS2019 and Visual Micro. Now that the groundwork has been laid, I would assume that this same strategy could be implemented for other IDE’s as well.

I have started GitHub repositories for the Teensy demo sketch TeensyOTADemo and my TeensyFlash C# command-line program on my GitHub site, and I would welcome suggestions/improvements.

10 October 2021 Update:

I’m trying to determine what caused the slow update when using my command-line program and the HC-05 when doing an OTA update relative to the situation when using Tera Term to manually transfer the .HEX file to the Teensy. In both cases, the OTA link is through the HC-05, so that module shouldn’t be the cause. However, just to check I went back and checked the HC-05 settings using the ‘AT Command Mode’ trick discussed in this post. AFAICT, the HC-05 was set to 115200 baud rate. I also checked to see if I had somehow misconfigured the serial port created in my command-line program, but it is set to create a port at 115,200 BPS as well. So, I’m officially baffled.

To investigate the problem, did an OTA update via the HC-05 BT link, but using Tera Term to trigger the update process and send the .HEX file. Using this method, it took approximately 14 sec to transfer the entire 3142-line .HEX file. Then I did an OTA update using my command-line program, using the same HC-05 BT link. With this setup, the transfer took approximately 35 seconds – over twice the time. Watching with my scope, it is clear that the transfer takes place at a much slower rate when using the command-line program, even though (at least AFAICT) the programmed baud rates are identical in both cases. The two photos below show the transfer rate for both cases, using the same time scale for both.

File Transfer using Command-line Program and HC-05 BT Link
File Transfer using Tera Term and HC-05 BT Link

After a bit of web research on C# .NET serial port programming, I came across this post, which indicates that using .NET for this task was not a very wise move – bummer!

Stay tuned,

Frank

Another try at Wall Offset Tracking

Leave a reply

Posted 11 September 2021

I’m making another try at getting wall offset capture and tracking right, after several failed attempts. This time I’m starting with a small ‘FourWD_WallTrackTest_V3’ project in VS2019. I think I actually got this working before, but an unfortunate laptop backup catastrophe cost me a couple of months of work, and it has taken me this long to recover. Here’s the current program:

With this code, I ran a test using PID values of 300,0,0 and an offset target of 40cm. The following short video and Excel plot show the results

Now all I have to do is make this same trick work for both the ‘inside offset’ and ‘outside offset’ starting conditions, and then for both the left-side and right-side tracking conditions, then I can integrate the code back into the main robot operating system.

18 September 2021 Update:

After getting the left-side tracking working reasonably well, I started working on right-side tracking, and discovered that what worked for the left side with no problems did not work particularly well for right-side tracking. After the usual number of programming mistakes I did get right-side tracking working, albeit with a significantly different PID parameter set. For left-side tracking, PID = (300,0,0) seemed to work OK, but for right-side tracking I wound up with PID = (300,0,300). Moreover, when I tried even very small (like 10) values for the I parameter, the robot lost track after just two or three oscillations. So, for the moment it looks like (300,0,0) for the left side, and (300,0,300) for the right side. Here’s a short video showing successful right-side tracking.

And here is the ‘finished’ FourWD_WallTrackTest_V3 code:

Stay tuned

Frank

Another Try at Wall Offset Tracking, Part II

Leave a reply

Posted 22 June 2021

In my previous post on this subject, I described my effort to improve Wall-E2’s wall tracking performance by leveraging controlled-rate turns and the dual VL53L0X ToF LIDAR arrays. This post describes an enhancement to that effort, aimed at allowing the robot to start from a non-parallel orientation and still capture and track a desired wall offset.

The previous post showed that when starting from a parallel orientation either inside or outside the desired wall offset, the robot would make a turn toward the offset line, move straight ahead until achieving the desired offset, then turn down-track and start tracking the desired offset. The parallel starting condition was chosen to make things easier, but of course that isn’t realistic – the starting orientation may or may not be known. In previous work I created an entire function ‘RotateToParallelOrientation()’ to handle this situation, but I would rather not have to do that. It occurred to me that I might be able to eliminate this function entirely by utilizing the known characteristics of the triple-VL53L0X array. The linear array exhibits a definite relationship between ‘steering value’, (the difference between the front and rear sensor values, divided by 100) and the off-perpendicular orientation of the array. At perpendicular (parallel orientation of the robot), this value is nominally zero, and exhibits a reasonably linear relationship out to about +/- 40º from perpendicular, as shown in the following plot from this post from a year ago.

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

As can be seen above, the ‘Steering’ value is reasonably linear, with a slope of about -0.0625/deg. So, for instance, the calculated value for an offset of 30cm would be -0.0625*30 = -0.1875, which is very close to the actual plotted value above for 30º

So, it should be possible to calculate the off-perpendicular angle of the robot, just from the measured steering value, and from that knowledge calculate the amount of rotation needed to achieve the desired offset line approach angle.

The desired approach angle was set more or less arbitrarily by

and this assumes an initial parallel orientation (i.e. 0º offset in the above plot). If, for instance, the initial steering value was -0.1875, indicating that the robot was pointing 30º away from parallel, then the code to compute the total cut (assuming we are tracking the wall on the left side of the robot) would look something like this:

So, for example, if the robot’s starting orientation was offset 30º away from the wall, and 20cm inside the desired offset line of 30cm, we would have:

cutAngleDeg = WALL_OFFSET_TGTDIST_CM – (int)(Lidar_LeftCenter / 10.f);

cutAngleDeg = 30 – 10 = 20

adjCutAngleDeg = cutAngleDeg – 30 = -10º, so the robot would actually turn 10º CCW (back toward the wall) before starting to move to capture the desired offset line.

If, on the other hand, the robot was starting from outside the desired offset (say at 60cm), but with the same (away from wall) pointing angle, then the result would be

cutAngleDeg = WALL_OFFSET_TGTDIST_CM – (int)(Lidar_LeftCenter / 10.f);

cutAngleDeg = 30 – 60 = -30

adjCutAngleDeg = cutAngleDeg – 30 = -30 – 30 = -60º, so the robot would actually turn 60º CCW (back toward the wall) before starting to move to capture the desired offset line.

26 June 2021 Update:

I modified my test program to just report the rear, center, and front VL53L0X distances, plus the steering value and the computed off-parallel angle, using the -0.0625 slope value obtained from the above plots. Unfortunately, the results were wildly unrealistic, leading me to believe something was badly wrong somewhere along the line. So, I redid the plots, thinking maybe the left side VL53L0X sensors were different enough to make that much of a difference. When I plotted the steering value vs off-parallel angle for 10, 20, 30 & 40cm wall offsets, I got a significantly different plot, as shown below:

As can be seen in the above plot, the slope of steering values to off-parallel angles is nice and linear, and also quite constant over the range of wall offset distances from 10 to 40 cm; this is quite a bit different than the behavior of the right-side sensor array, but it is what it is. In any case, it appears that the slope is close to 1.4/80 = 0.0175, or almost twice the right-side slope derived from the previous right-side plots.

Using the value of 0.0175 in my GetSteeringAngle() function results, and comparing the calculated off-parallel angle with the actual measured angle, I get the following Excel plot.

As can be seen in the above plot, the agreement between measured and calculated off-parallel angles is quite good, using the average slope value of 0.0175.

The above plot shows the raw values that produced the first plot.

So, back to my ‘FourWD_WallTrackTest’ program to develop Wall-E2’s ability to capture and then track a particular desired offset. In my first iteration, I always started with Wall-E2 parallel to the wall, and calculated an intercept angle based on the difference between the robot’s actual and desired offsets from the near wall. This worked very nicely, but didn’t address what happens if the robot doesn’t start in a parallel orientation. However, when I tried to use the data from a previous post, the results were wildly off. Now it is time to try this trick again, using the above data instead. Because the measured steering value/off angle slopes for all selected wall offsets were essentially identical, I can eliminate the intermediate step of calculating the appropriate slope value based on the current wall offset distance.

So, I modified my ‘getSteeringAngle()’ function to drop the ‘ctr_dist_mm’ parameter and to use a constant 0.0175 slope value, as shown below:

With this modification, I was able to get pretty decent results; Wall-E2 successfully captured and tracked the desired offsets from three different ‘inside’ (robot closer to wall than the desired offset) orientations, as shown in the following short videos:

‘Inside’ capture, with initial orientation angle > desired approach angle
‘Inside’ capture with initial orientation angle < desired approach angle
‘Inside’ capture with negative initial orientation angle

04 July 2021 Update:

After succeeding with the ‘inside’ cases, I started working on the ‘outside’ ones. This turned out to be considerably more difficult, as the larger distances from the wall caused considerable variation in the VL53L0X measurements (lower SNR?), which in turn produced more variation in the starting and ‘cut’ angles. However, the result does seem to be reasonably reliable, as shown in the following videos.

‘outside’ capture with initial outward angle
‘outside’ capture with initial inward angle
‘outside’ capture with small outward angle.

05 July Update:

After getting the left-side tracking algorithm working reasonably well, I ported the ‘TrackLeftWallOffset()’ functionality to ‘TrackRightWallOffset()’. After making (and mostly correcting) the usual number of mistakes, I got it going reasonably well, as shown in the following short videos:

Right wall tracking, starting inside desired offset, oriented toward wall
Right wall tracking, starting inside desired offset, oriented away from wall
Right wall tracking, starting outside desired offset, oriented toward wall
Right wall tracking, starting outside desired offset, oriented away from wall

Here is the complete code for my wall capture/track test program:

Here is a link to the above file, plus all required library & ancillary files.

Stay tuned,

Frank

Another Try at Wall Offset Tracking

Leave a reply

Posted 22 June 2021

About nine months ago (October 2020) I made a run at getting offset tracking to work (see here and here). This post describes yet another attempt at getting this right, taking advantage of recent work on controlled-rate turns. I constructed a short single-task program to do just the wall tracking task, hopefully simplifying things to the point where I can understand what is happening.

One of the big issues that arose in previous work was the inability to synch my TIMER5 ISR with the PID library’s ‘Compute()’ function. The PID library insists on managing the update timing internally, which meant there was no way to ensure that Compute() would be called every time the ISR ran. I eventually came to the conclusion that I simply could not use the PID library version, and instead wrote my own small function that did the Compute() function, but with the timing value passed in as an argument rather than being managed internally. This forces the PID calculations to actually update in synch with the TIMER5 interrupt. Here’s the new PIDCalcs() function:

As can be seen from the above, this is a very simple routine that just does one thing, and doesn’t incorporate any of the improvements (windup suppression, sample time changes, etc) available in the PID library. The calling function has to manage the persistent parameters, but that’s a small price to pay for clarity and the assurance that the output value will indeed be updated every time the function is called.

With this function in hand, I worked on getting the robot to reliably track a specified offset, but was initially stymied because while I could get it to control the motors so that the robot stayed parallel to the nearest wall, I still couldn’t get it to track a specific offset. I solved this problem by leveraging my new-found ability to make accurate, rate-controlled turns; the robot first turns by an amount proportional to it’s distance from the desired offset, moves straight ahead until the offset is met, and then turns the same number of degrees in the other direction. Assuming the robot started parallel to the wall, this results in it facing the direction of travel, at the desired offset and parallel to the nearest wall. Here is the code for this algorithm.

Here are a couple of videos showing the ability to capture and track a desired offset from either side.

In both of the above videos, the desired wall offset is 30 cm.

Stay Tuned,

Frank

Turn Rate PID Tuning, Part IV

Leave a reply

Posted 10 June 2021,

In my last post on this issue, I described using a small test program to explore an in-line version of the PID (Proportional-Integral-Differential) algorithm for turn rate control with Wall-E2, my autonomous wall-following robot. This post describes some follow-on work on this same subject.

The fundamental problem with all the available Arduino PID libraries is they all require the user to wait in a loop for the PID::Compute() function to decide when to actually produce a new output value, and since this computation is inside the function, it is difficult or impossible to synchronize any other related timed element with the PID function. In my case where I want to control the turn rate of a robot, the input to the PID engine is, obviously, the turn rate, in degrees/sec. But, calculation of the turn rate is necessarily a timed function, i.e. (curent_heading – last_heading) / elapsed_time, where the ‘elapsed_time’ parameter is usually a constant. But, all the Arduino PID libraries use an internal private class member that defines the measurement period (in milliseconds), and this value isn’t available externally (well, it is, but only because the user can set the sample time – it can’t be read). So, the best one can do with the current libraries is to use the same constant for PID::SetSampleTime() and for any external time-based calculations, and hope there aren’t any synchronization issues. With this setup, it would be quite possible (and inevitable IMHO) for the PID::Compute() function to skip a step, or to be ‘phase-locked’ to producing an output that is one time constant off from the input.

The solution to this problem is to not use a PID library at all, and instead place the PID algorithm in-line with the rest of the code. This ensures that the PID calculation any related time-based calculations are operating on the same schedule. The downside of this arrangement is loss of generality; all the cool enhancements described by Brett Beauregard in his wonderful PID tutorial go away or have to be implemented in-line as well. From Brett’s tutorial, here’s ‘the beginner’s PID algorithm’:

The difficulty with all the current PID libraries is the ‘dt’ parameter in the above expression; the implementation becomes much easier if ‘dt’ is a constant – i.e. the time between calculations is a constant. However, this constraint also requires that the library, not the user program, controls the timing. This just doesn’t work when the ‘Input’ parameter above also requires a constant time interval for calculation. In my case of turn rate control, the turn rate calculation requires knowledge of the time interval between calculations, and the calculation itself should be done immediately after the turn rate is determined, using the same time interval. So, the turn rate is calculated and then PID::Compute() is called, but Compute() may or may not generate a new output value, because it can return without action if it’s internal time duration criteria isn’t met; see the problem? It may, or even might, generate a new output value each time, but there is no way to ensure that it will!

After figuring this out the hard way (by trying and failing to make the library work), I finally decided to forget the library – at least for my turn rate problem, and in-line all the needed code. Once I had it all running, I abstracted just the PID algorithm to its own function so I could use it elsewhere. This function is shown below:

As you can see, the explanatory comments are much bigger than the function itself, which is really just eight lines long. Also, it has a huge number of arguments, five of which are references that are updated by the function. This function wouldn’t win any awards for good design, as it has too many arguments (wide coupling), but it does have high cohesion (does just one thing), and the coupling is at least ‘data’ coupling only.

Once this function was implemented, the calling function (in this case ‘SpinTurn()’ looks like this:

In the above code, the lines dealing with ‘TIMSK5’ are there to disable and then re-enable the TIMER5 interrupt I have set up to update external sensor values every 100 mSec. I’m not really sure that this HAS to be done, but once I learned how to do it I figured it wouldn’t hurt, either ☺

Now that I have this ‘PIDCalcs()’ function working properly, I plan to use it in several other places where I currently use the PID library; it’s just so much simpler now, and because all the relevant parameters are visible to the calling program, debugging is now a piece of cake where before it was just an opaque black box.

12 June 2021 Update:

After chasing down and eliminating a number of bugs and edge-case issues, I think I now have a pretty stable/working version of the ‘SpinTurn’ function, as shown below:

With this code in place, I made some 180º turns at 45 & 90 deg/sec, both on my benchtop and on carpet, as shown in the plots and video below:

Average turn rate = 41.8 deg/sec
Average turn rate = 86 deg/sec
Average turn rate = 44.8 deg/sec
Average turn rate = 90.1 deg/sec

Stay Tuned!

Frank

Turn Rate PID Tuning, Part III

Leave a reply

In my previous post on this subject, I described my efforts to control the turn rate (in deg/sec) of my two-wheel robot, in preparation for doing the same thing on Wall-E2, four wheel drive autonomous wall following robot.

As noted previously, I have a TIMER5 Interrupt Service Routine (ISR) set up on my four wheel robot to provide updates to the various sensor values every 100 mSec, but was unable to figure out a robust way of synchronizing the PID library’s Compute() timing with the ISR timing. So, I decided to bag the PID library entirely, at least for turn rate control, and insert the PID algorithm directly into the turn rate control, and removing the extraneous stuff that caused divide-by-zero errors when the setSampleTime() function was modified to accept a zero value.

To facilitate more rapid test cycles, I created a new program that contained just enough code to initialize and read the MP6050 IMU module, and a routine called ‘SpinTurnForever() that accepts PID parameters and causes the robot to ‘spin’ turn forever (or at least until I stop it with a keyboard command. Here’s the entire program.

This program includes a function called ‘CheckForUserInput()’ that, curiously enough, monitors the serial port for user input, and uses a ‘switch’ statement to execute different commands. One of these commands (‘q’ or ‘Q’) causes ‘SpinTurnForever()’ to execute, which in turn accepts a 4-paremeter input that specifies the three PID parameters plus the desired turn rage, in deg/sec. This routine then starts and manages a CCW turn ‘forever’, in the ‘while()’ block shown below:

This routine mimics the PID library computations without suffering from library’s synchronization problems, and also allows me to fully instrument the contribution of each PID term to the output. This program also allows me to vary the computational interval independently of the rest of the program, bounded only by the ability of the MPU6050 to produce reliable readings.

After a number of trials, I started getting some reasonable results on my benchtop (hard surface with a thin electrostatic mat), as shown below:

Average turn rate = 89.6 deg/sec

As can be seen in the above plot, the turn rate is controlled pretty well around the 90 deg/sec turn rate, with an average turn rate of 89.6 deg/sec.

The plot below shows the same parameter set, but run on carpet rather than my bench.

Average turn rate = 88.2 deg/sec

Comparing these two plots it is obvious that a lot more motor current is required to make the robot turn on carpet, due to the much higher sideways friction on the wheels.

The next step was to see if the PID parameters for 90 deg/sec would also handle different turn rates. Here are the plots for 45 deg/sec on my benchtop and on carpet:

Average turn rate = 44.7 deg/sec
Average turn rate = 43.8 deg/sec

And then 30 deg/sec on benchtop and carpet

Average turn rate = 29.8 deg/sec
Average turn rate 28.8 deg/sec

It is clear from the above plots that the PID values (5,0.8,0.1) do fairly well for the four wheel robot, both on hard surfaces and carpet.

Having this kind of control over turn rate is pretty nice. I might even be able to do turns by setting the turn rate appropriately and just timing the turn, or even vary the turn rate during the turn. For a long turn (say 180 deg) I could do the first 90-120 at 90 deg/sec, and then do the last 90-60 at 30 deg/sec; might make for a much more precise turn.

All of the above tests were done with a 20 mSec time interval, which is 5x smaller than the current 100mSec time interval used for the master timer in Wall-E2. So, my next set of tests will keep the turn rate constant and slowly increase the time interval to see if I can get back to 100 mSec without any major sacrifice in performance.

28 May 2021 Update:

I went back through the tests using a 100 mSec interval instead of 20 mSec, and was gratified to see that there was very little degradation in performance. The turn performance was a bit more ‘jerky’ than with a 20 mSec interval, but still quite acceptable, and very well controlled, both on the benchtop and carpet surfaces – Yay! Here are some plots to show the performance.

Average turn rate = 29.7 deg/sec
Average turn rate = 28.4 deg/sec
Average turn rate = 44.4 deg/sec
Average turn rate = 43.0 deg/sec
Average turn rate = 89.7 deg/sec
Average turn rate = 86.6 deg/sec

31 May 2021 Update:

I made some additional runs on benchtop and carpet, thinking I might be able to reduce the turn-rate oscillations a bit. I found that by reducing the time interval back to 20 mSec and increase the ‘D’ (differential) parameter. After some tweaking back and forth, I wound up with a PID set of (5, 0.8, 3). Using these parameters, I got the following performance plots.

Average turn rate = 87.3 deg/sec
PID = (5,0.8,3), 20mSec interval, 90 deg/sec

As can be seen in the Excel plot and the movie, the turn performance is much smoother – yay!

Stay tuned!

Frank

New Batteries for Wall-E2

Leave a reply

I have been using a set of four Panasonic 18650 LiPo batteries in a 2-cell stack configuration in Wall-E2, my autonomous wall following robot. for a little over three years now, and they are starting to show their age. So, I decided to replace these

with these

Panasonic 18650G-A 3500 mAH 3.7V LiPo battery

Three years younger, and with another 100 mAH (rated, at least).

Here’s a photo record of the process of changing out the batteries

Old batteries still installed (can tell by the green showing through the ‘V5’ cutout)
Old batteries before replacement
  • Battery connection details
  • Replacement battery connection details
New batteries installed in holder

Stay tuned,

Frank

Turn Rate PID Tuning, Part II

Leave a reply

Posted 14 May 2021,

In my previous post on this topic, I described my efforts to use the Arduino PID library to manage turns with Wall-E2, my autonomous wall following robot. This post talks about a problem I encountered with the PID library when used in a system that uses an external timing source, like the TIMER5 ISR in my system and a PID input that depends on accurate timing, such as my turn-rate input.

In my autonomous wall-following robot project, I use TIMER5 on the Arduino Mega 2560 to generate an interrupt ever 100 mSec, and update all time-sensitive parameters in the ISR. These include results from all seven VL53L0X ToF distance sensors, the front-mounted LIDAR, and heading information from a MP6050 IMU. This simplifies the software immensely, as now the latest information is available throughout the code, and encapsulates all sensor-related calls to a single routine.

In my initial efforts at turn-rate tuning using the Arduino PID library, I computed the turn rate in the ISR by simply using

This actually worked because, the ISR frequency and the PID::Compute() frequency were more or less the same. However, since the two time intervals are independent of each other there could be a phase shift, which might drift slowly over time. Also, if either timer interval is changed sometime down the road, the system behavior could change dramatically. I thought I had figured out how to handle this issue by moving the turn-rate computation inside the PID::Compute() function block, as shown below

In a typical PID use case, you see code like the following:

After making the above change, I started getting really weird behavior, and all my efforts at PID tuning failed miserably. After a LOT of troubleshooting and head-scratching, I finally figured out what was happening. In the above code configuration, the PID generates a new output value BEFORE the new turn rate is computed, so the PID is always operating on information that is at least 100mSec old – not a good way to run a railroad!

Some of the PID documentation I researched said (or at least implied) that by setting the PID’s sample time to zero using PID::SetSampleTime(0), that Compute() would actually produce a new output value every time it was called. This meant that I could do something like the following:

Great idea, but it didn’t work! After some more troubleshooting and head-scratching, I finally realized that the PID::SetSampleTime() function specifically disallows a value of zero, as it would cause the ‘D’ term to go to infinity – oops! Here’s the relevant code

As can be seen from the above, an argument of zero is simply ignored, and the sample time remains unchanged. When I pointed this out to the developer, he said this was by design, as the ‘ratio’ calculation above would be undefined for an input argument of zero. This is certainly a valid point, but makes it impossible to synch the PID to an external master clock – bummer!

After some more thought, I modified my copy of PID.cpp as follows:

By moving the SampleTime = (unsigned long)NewSampleTime; line out of the ‘if’ block, I can now set the sample time to zero without causing problems with the value of ‘ratio’. Now PID::Compute() will generate a new output value every time it is called, which synchs the PID engine with the program’s master timing source – yay!

I tried out a slightly modified version of this technique on my small 2-wheel robot. The two-wheeler uses an Arduino Uno instead of a Mega, so I didn’t use a TIMER interrupt. Instead I used the ‘elapsedMillisecond’ library and set up an elapsed time of 100 mSec, and also modified the program to turn indefinitely at the desired turn rate in deg/sec.

I experimented with two different methods for controlling the turn rate – a ‘PWM’ method where the wheel motors are pulsed at full speed for a variable pulse width, and a ‘direct’ method where the wheel motor speeds are varied directly to achieve the desired turn rate. I thought the PWM method might work better on a heavier robot for smaller angle turns as there is quite a bit of inertia to overcome, but the ‘direct’ method might be more accurate.

Here’s the code for the ‘direct’ method, where the wheel speeds are varied with

SetLeftMotorDirAndSpeed

Here’s the code for the PWM method: the only difference is that is the duration of the pulse that is varied, not the wheel speed.

Here’s a short video showing the two-wheel robot doing a spin turn using the PWM technique with a desired turn rate of 90 deg/sec, using PID = (1,0.5,0).

The average turn rate for the entire run was about 85 deg/sec.

Here’s another run, this time on carpet:

Average turn rate for the entire run was about 85 deg/sec

Here’s some data from the ‘direct’ method, on hard flooring

Average turn rate was ~ 85 deg/sec

And on carpet

Average turn rate ~83 deg/sec

So, it appears that either the PWM or ‘direct’ methods are effective in controlling the turn rate, and I don’t really see any huge difference between them. I guess the PWM method might be a little more effective with the 4-wheel robot caused by the wheels having to slide sideways while turning.

Stay Tuned!

Frank

Wall Parallel Find PID Tuning

Leave a reply

Posted 10 April 2021

In addition to using PID for homing to its charging station and for turn rate control, Wall-E2 also uses PID for finding the parallel orientation to a nearby wall. After successfully tuning the turn rate and IR Homing PID controllers using the Ziegler-Nichols method for PID tuning, I decided to see what I could do with the PID controller for parallel orientation finding

Wall-E2 uses two 3-element VL53L0X Time-of-Flight distance sensors for parallel orientation finding. The idea is that when all three sensors report the same distance, then the robot must be oriented parallel to the wall. The Teensy 3.5 Array Controller MCU calculates a ‘steering value’ using the expression (shown for the left side array):

This value is fed to the PID engine, which drives the motors to zero it out – thus arriving at a parallel orientation. Originally I just basically ‘winged it’ in choosing PID Kp, Ki & Kd values, arriving emperically at Kp = 200, Ki = 50, Kd = 0. However, after going through the K-N process with Wall-E2’s other two PID control setups, I decided to try it with this one as well.

The first step is to determine Kc, the Kp value for which the system oscillates in a reasonably stable fashion. To accomplish this I started with Kp = 20 and worked my way up in stages, plotting the ‘steering value’ each time. The last three trials (as shown in the following plots) were for Kp = 400, Kp = 500 and Kp = 600:

Looking at the above plots, it looks like Kp = 600 will work for Kc. Using the K-N formula, we get

Using the above values for the Parallel Find PID, we get the following plot:

Which is not exactly what I thought it would be – it looks like my guess for Kc must be off. Trying again with a Kc = 400 –> PID = 200,180,240, we get:

which, to my eye at least, seems a bit better.

To test how this worked with ‘real’ parallel finding, I incorporated these parameter values into my ‘RotateToParallelOrientation()’ routine and ran a couple of tests. Here’s one where Wall-E2 starts in the ‘toed-out’ position:

And here’s the Excel plot from this same run

As can be seen, the robot takes less than two seconds to converge on a pretty decent parallel orientation, starting from a 30-40º angle to the near wall.

Here’s another run where the robot starts in the ‘toed-in’ orientation.

And here’s the Excel plot for the run

Again, the robot gets to a pretty decent parallel orientation within 2 seconds of the start of the run. The only concern I have with this run is that it winds up pretty close to the wall.

Serial Bridge over WiFi using ESP32 DevKitC

Leave a reply

Posted 09 April 2021

I have been wanting to upgrade my robot’s brains from the old Arduino Mega 2560 to a more modern Teensy 3.x or 4.x MCU for some time now, but have been stymied by the lack of Over-The-Air (OTA) programming support. My current Mega-based setup uses a pair of Pololu Wixels to form a transparent wireless bi-directional serial link between my PC and the robot. My Microsoft Visual Studio IDE sees the link as just another COM port, and the Mega thinks it’s Tx/Rx0 lines are connected directly to the PC; couldn’t be simpler and more effective. The range of the Wixel connection is on the order of a few tens of meters, but that’s all I need for managing my autonomous wall-following robot.

Unfortunately, the Teensy world seems a bit short on effective wireless OTA solutions, or at least my searches have come up mostly dry. Some users report they have been able to use a Raspberry Pi Zero-W for this purpose, but that seems like a major overkill

After yet another Google search, I started seeing some posts about the ability to form a wireless serial data bridge using an ESP32 wireless-enabled development module, and I since I happened to have a ESP32 DevKitC hanging around Idecided to try my hand at that, maybe as a first step to achieving OTA nirvana with a Teensy 3/4.x.

The starting point for this project was this tutorial (the bottom ‘Serial Bridge Using ESP8266 (Simpler)’ part), based on this module sold on the AliExpress site. The AliExpress module wasn’t exactly the same as the DevKitC I had on hand, but it was close enough and AFAICT the pinouts are identical.

The first step was getting the Arduino/VS2019 IDE to recognize the ESP32 hardware. This was a semi-major PITA, but I eventually found some posts showing how the board information could be added to the system.

The next step was getting PuTTY downloaded and installed on my Win10 system. This went fairly easily.

Next I downloaded the ESP32 serial bridge software from the GitHub site and set it up as a project in my VS2019/Visual Micro IDE. This all seemed to work, and I was able to upload the program to my DevKitC module

I also happened to have an old CKdevices FTDI module, so I was able to connect to the required serial lines on the ESP32 module. However, I discovered that the tutorial didn’t use the standard Tx/Rx lines shown in the DevKitC pinout diagram; evidently they had to be moved to allow all three UART/USB modules to be connected at the same time. The actual pinouts used by the tutorial are:

After some fumbling around, I finally realized that the ‘GPIOxx’ labels in the above pinout list is the same as the numbers printed on the actual module, i.e. ‘GPIO21′ <==> ’21’ on the module PCB (the lone exception to this is ‘GPIO1’, which corresponds to ‘TX’ on the module). Here’s a picture from the GitHub documentation showing the actual layout expected by the sofware.

After working my way though these problems, here’s the physical setup I wound up with:

CkDevices FTDI UART/USB connected to COM5 on USB side, Tx2/Rx2 on ESP32 side
Detail showing the pin connections to ESP32 module. Yellow is connected to ESP32 Rx2, Green to Tx2

With the above hardware setup, I was able to pass serial data from one PuTTY terminal connected to the 192.168.4.1/8882 port, and another connected to COM5 on the PC (corresponding to COM2 on the ESP32).

Having accomplished this, I’m still unsure how to go about using this capability to program a Teensy 3/4.x using the wireless link. More study required!

Frank