Tag Archives: robots

Wall-E3 Right Wall Following Trial

Leave a reply

Posted 23 March 2022,

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

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

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

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

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

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

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

25 March 2022 Update:

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

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

Stay tuned,

Frank

Wall-E3 Time Required for All-Sensor Update

Leave a reply

Corralling all sensor data updates into one place:

I have been struggling with how to manage sensor data updates for Wall-E3. When I originally started working with Wall-E, it had only three sensors – a left and right-side HC-04 ‘ping’ sensor and the front LIDAR distance sensor, so data updates weren’t a significant part of the algorithm. Since then the sensor population as ballooned past the double-digit mark, with seven VL53L0X side/rear distance sensors, the front LIDAR sensor, two high-side current sensors, and the MPU6050 IMU.

Back in August 2020 I decided to change from a ‘request only when needed’ to a TIMER interrupt-based sensor update paradigm. The idea was to update all sensor data X times/sec in an Interrupt Service Routine (ISR). This worked great, but caused other problems that eventually led me to abandon this approach. In addition to not knowing exactly when/where in the program the sensor data changed, it appeared this approach was incompatible with my use of the PID library for motion control. The PID library’s ‘Compute()’ function expects to be called in a loop that runs many times faster than the PID’s internal update period (100mSec by default). PID::Compute() returns without doing anything until its internal 100mSec timer expires, at which point it does one PID computation and then resets the timer. So, there was a conflict, because I wanted to call PID::Compute() each time the TIMER ISR executed (using a ‘global’ boolean flag), but PID::Compute() wants to execute only when it’s internal timer expires. I never could figure out how to make those two requirements work together. Eventually I abandoned both of them. First, I dumped the PID library and rolled my own PIDCalcs() function that computed a new output every time it was called, and I dumped the timer ISR in favor of ‘just in time’ sensor data updates.

Fast-forward to the present, and now I’m still struggling to figure out how to manage sensor data updates. As my latest ‘sand-box’ testing showed, I need to update all the distance sensors even when I’m only tracking one side, so just updating one side or the other doesn’t work. So, I created a ‘UpdateAllDistances()’ function as shown below:

This function also causes the front and rear distance arrays to be updated and new front/rear variances to be calculated. The function is intended to be called from both the left and right wall tracking loops, and anywhere else updated distance and related data updates are required.

Having created this function, the next question becomes – how long does this function take to execute? If it is too long, then tracking performance will suffer. To answer this question, I placed code at the beginning and end of UpdateAllDistances() to toggle a hardware pin so I can measure the elapsed time on a scope, and I placed code at the beginning/end of a small WALL_TRACK_UPDATE_INTERVAL_MSEC test loop in setup(), as follows:

With this test, I got the following output on my HANMATECK DOS1102 digital O’Scope:

200mSec tracking loop (blue) and UpdateAllEnvironmentParameters() duration (yellow)

As can bee seen in the above plot, UpdateAllEnvironmentParameters() takes around 6-10mSec to update all environmental parameters (basically everything except MPU6050 heading), leaving 190-194mSec to complete the rest of the tracking update loop. This is very good news, as it means I can basically think of UpdateAllEnvironmentParameters() as a one-line ‘do everything’ command with negligible duration.

Next I made the same measurement, but this time with the actual ‘TrackLeftWallOffset() code being executed. As can be seen from the following image, the result is essentially identical to the first test; UpdateAllEnvironmentParameters() takes around 6-10mSec.

200mSec tracking loop (blue) and UpdateAllEnvironmentParameters() duration (yellow)

Then I did this test one more time, except this time I toggled the blue trace at the beginning and end of wall track processing, to show the time remaining in the 200mSec loop. Here’s what actually happens in the tracking loop:

And here’s the screen grab from my O’scope showing the actual duration of everything in the above loop.

tracking loop processing duration (blue) and UpdateAllEnvironmentParameters() duration (yellow)

As can be seen, almost all of the tracking processing time is spent in UpdateAllEnvironmentParameters(), and there is plenty of time to do additional processing (like anomaly handling). The 200 mSec loop is denoted above by adjacent rising edges of the blue trace, and all processing is finished at the trailing edge of the blue trace, so only about 10mSec, or about 5%, of the 200mSec is taken.

So it is clear that consolidating all environmental sensor updates into one function is a big winner. The time taken for sensor data updates is a small percentage of the time available for the entire tracking loop, but it is almost all of the time required in each tracking loop. This is very interesting result. The time required for sensor update probably cannot be reduced, as it depends on the actual hardware sensor response times and the ability to get the sensor data back to the main Teensy 3.5 processor via I2C. However, it now appears that I could easily reduce the overall tracking loop duration from the nominal 200mSec to 100, 50, or even 20mSec with no adverse effects, and presumably a corresponding increase in tracking performance. This is probably the biggest win associated with the change from the Arduino MEGA2560 to the Teensy 3.5 – so much less time required for processing.

Stay tuned,

Frank

Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part VIII

Leave a reply

Posted 19 February 2022,

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

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

22 February 2022 Update:

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

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

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

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

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

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

02 March 2022 Update:

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

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

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

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

Stay Tuned,

Frank

Using Wire1 & Wire2 with the I2CDevLib & MPU6050 Libraries

2 Replies

While working on porting Wall-E2’s ‘second deck’ hardware (VL53L0X array and LIDAR) to Wall_E3, I started running into problems associated with using Wire1 on the main Teensy 3.5 master processor to communicate with the Teensy 3.5 slave processor that manages the seven VL53L0X time-of-flight distance sensors. As I worked to troubleshoot the issue, it soon became evident that “I wasn’t in Kansas anymore”, and in fact had once again gone down a rabbit hole into Wonderland, with nary a bread-crumb in sight. Basically, I have been trying to use both the ‘i2c_t3.h’ and ‘Wire.h’ Teensy to utilize multiple I2C buses (Wire1, Wire2, etc) over the last few years, without really understanding what I was doing. In the process I have created a spaghetti mess of conflicting I2C library file locations and configurations.

So, this post is my account of what went wrong, and the steps taken to get things working properly again.

The problem – utilizing multiple I2C buses:

Teensy 3.x processors provide for multiple I2C buses, a feature I used originally to manage the 7-element VL53L0X array located on the ‘second deck’ of Wall_E2, my autonomous wall following robot. With the addition of a Teensy 3.5 for the main robot processor, the multiple I2C bus problem is now relevant to the main processor as well. The main processor now uses Wire to talk to the second-deck VL53L0X array manager (Teensy 3.5), and Wire1 for the IR homing beacon detector/demodulator and the MPU6050 IMU. Consequently, the main processor must utilize (multi-wire) capable library functions.

The 7-element VL53L0X array manager (Teensy 3.5)

This worked great, even though I had tried my best to screw it up. The main project file has already been changed to use <Wire.h> and although it uses a local copy of I2C_Anything.h, that copy uses <Wire.h> as well. Also, VL53L0X.h (local folder copy) also uses <Wire.h>. So, I made the following changes:

  • Removed VL53L0X.h/cpp and I2C_Anything.h from project references and deleted the local file copies
  • Changed #include “file.h” to <file.h> for both (not sure this is necessarsy
  • Deleted the vl53l0x-arduino folder from the \Libraries folder so there would be only one version of VL53L0X.h/cpp available.
  • Re-scanned for libraries, did a File->SaveAll, and recompiled OK – yay!

So now at least the Teensy_7VL53L0X_Slave_V3 project has been cleaned up

Main Wall-E3 Processor (Teensy 3.5)

This is where all the trouble with multiple I2C busses started. The main processor has to talk to the VL53L0X array manager via I2C on Wire1, which means that not only does the main processor code need to utilize multi-bus functionality, but I2C_Anything (which internally uses Wire for bit-wise comms) does as well. In addition, interfacing to the MPU6050 requires the use of Jeff Rowberg’s I2CDevLib stuff, specifically MPU6050_6Axis_MotionApps_V6_12.h, I2CDev.h, I2C_Anything.h, and a Wire1 capable version of I2C_Anything.h. To make all this work, I made the following changes:

The #include for MPU6050_6Axis_MotionApps_V6_12.h, I2CDev.h is aimed at \Libraries\MPU6050\, which is a (old) copy of C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050\. A suggestion in Jeff Rowberg’s ReadME file regarding the use of symlinks instead of actual copies led me to this ‘how-to’ page on creating symlinks in Windows. So I deleted the C:\Users\paynt\Documents\Arduino\Libraries\MPU6050\ folder and instead created a ‘hard’ symlink from there to C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050\. Then I similarly deleted the C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev\ folder and created a ‘hard’ symlink from there to C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev\.

Here are the cmdline commands for both operations:

and here is the result of dir /A in the C:\Users\paynt\Documents\Arduino\Libraries\ folder

showing that the hard links were actually created properly.

So now when I right-click on include “MPU6050_6Axis_MotionApps_V6_12.h”, the file opens properly, and the location is shown as in the C:\Users\paynt\Documents\Arduino\Libraries\MPU6050 folder even though the file actually resides in C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050\. Similarly, for include “I2Cdev.h” the file opens properly and the location is shown as C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev\ even though it is actually in the C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev\ folder

This all worked, except now I’m getting errors that say that ‘I2C_PINS_18_19’ (and all the other Teensy I2C-specific enums) can’t be found – argggggghhhhh! They don’t seem to be defined anywhere in the C:\Program Files (x86)\Arduino\hardware\teensy\avr\ folder tree either – I’m at a loss

Well, maybe not. I’m beginning to think that the enums are <i2c_t3.h>-specific, and a more basic style of initialization is used with <Wire.h>. I sent off a plea to the Teensy forum – we’ll see.

In the meantime, I tried a couple of simple experiments that determined pretty conclusively that the <Wire.h> style does indeed work, but in a different (more constrained?) way than with <i2c_t3.h>. I created a ‘Wire_Slave_Sender’ VS2022 project by copying the ‘slave_sender.ino’ example and loaded it onto a T3.2. Then I created a ‘Wire_Master_Reader’ VS2022 project by copying the ‘master_reader.ino’ example, and loaded it onto a T3.5. With this setup I was able to demonstrate that ‘Wire.begin()’ facilitated I2C comms on pins 18 & 19 (the default pinouts for Wire0) on the T3.5 (pins 18 & 19 on the slave didn’t change), and ‘Wire1.begin()’ facilitated the same thing, but this time using pins 37 & 38 (the default pinouts for Wire1). Here are the programs:

and here’s a sampling of the output:

OK, now that I understand the <Wire.h> vs <i2c_t3.h> issues, I still have at least one more hurdle to clear. It appears that ” MPU6050_6Axis_MotionApps_V6_12.h ” is an older version of the DMP-enabled code for the MPU6050, and “MPU6050_6Axis_MotionApps612.h” (no ‘V’, no underscores in version number) is the latest and greatest. However, using this version also requires the correct version of i2cdev.h (I think). In any case, I was able to change the #includes on my T35_WallE3_V5 project to “MPU6050_6Axis_MotionApps612.h” and “I2Cdev.h” and get the program to compile for a T3.5 target. Whether or not it will actually behave as required is TBD.

To pursue this issue, I set up a simple T3.5 – MPU6050 plugboard configuration, and loaded an old MPU6050 test project – “Teensy_MPU6050_DMP6_V3”. This project uses the older “MPU6050_6Axis_MotionApps_V6_12.h” code (located in the project folder) along with I2Cdev.h/cpp, helper_3dmath.h, and MPU6050.h/cpp all in the project folder. It compiled right out of the box, and I was able to demonstrate successful interfacing with the MPU6050.

Next, I changed #include “MPU6050_6Axis_MotionApps_V6_12.h” to #include “MPU6050_6Axis_MotionApps612.h” whereupon it blew a whole bunch of compile errors. I was able to confirm that, due to the ‘hard’ symlink magic, the compiler thinks the “MPU6050_6Axis_MotionApps612.h” file is at …\Arduino\Libraries\MPU6050 rather than way down in the i2cdevlib tree – yay!

At this point, rather than continuing to modify the Teensy_MPU6050_DMP6_V3 project, I decided to create a Teensy_MPU6050_DMP6_V4 project and make all the changes there, so that when I screw up and get lost I can go back and start all over (ask me how I know to do this….). So I changed the include back to #include “MPU6050_6Axis_MotionApps_V6_12.h” and verified it still compiled OK, then I created a new project called Teensy_MPU6050_DMP6_V4 and copy/pasted the entire .ino file into it.

When I tried to compile the new project, it blew a bunch of errors about MPU6050_Base:: functions not being found. This sounds like the i2cdev.h/cpp that is being found is the later one, so I went ahead and changed #include “MPU6050_6Axis_MotionApps_V6_12.h” to #include “MPU6050_6Axis_MotionApps612.h” without doing anything else. This time it didn’t blow any errors about MPU6050_Base:: functions but did blow some similar to ‘I2C_PULLUP_EXT was not declared’. I think this is due to using <Wire.h> in the #include chain rather than <i2c_t3.h>, so I changed

With just that one change, the project now compiles for a Teensy 3.5 target – woohoo! In addition, I didn’t have to add any references to the project, which I have had to do in almost every other case – double woohoo!

Then I uploaded this project to the T3.5, and it actually worked – the MPU6050 is generating valid azimuth values – triple woohoo!

So now I have a working program using the latest/greatest DMP-enabled driver for the MPU6050, and a much simpler #include file/reference setup. Compare this:

to this:

Just for grins, I modified the _V4 project to see if I can move the MPU6050 from Wire (pins 19,18) to Wire1 (pins 37,38). This requires changing MPU6050 mpu to MPU6050((uint8_t) 0x68, &Wire1) and Wire.begin to Wire1.begin(). This worked like a champ, so at this point I think I have figured out all I need to know on this subject.

20 January 2022 – One last piece of the puzzle:

I use Nick Gammon’s wonderful I2C_Anything library (just two template functions, but…) a lot, but it doesn’t support multiple I2C buses. So, I decided to see if I could modify his template functions to accept a default argument that if present, specifies which I2C bus to use. Here’s the modified file, temporarily renamed to ‘I2C_AnythingMultiWire.h’

I used my previously constructed ‘Wire_Slave_Sender.ino’ and ‘Wire_Master_Reader.ino’ projects to test this, and it seems to work just as advertised. In ‘Wire_Slave_Sender.ino’ I use:

Which automagically uses ‘Wire’ because the 2nd argument is missing from the call, and in ‘Wire_Master_Reader.ino’ I use:

Which causes Wire1 to be used. Here are both demo programs in their entirety, along with the full ‘I2C_AnythingMultiWire.h’ file:

I made a ‘pull request’ to the I2C_Anything github repo so that everyone who uses it can benefit, but in the meantime feel free to use the I2C_AnythingMultiWire.h file.

25 January Update: Just one more ‘one last piece of the puzzle’

When I was using the ‘i2c_t3.h’ library, I noticed that I didn’t have to use external pullup resistors as long as I used the ‘I2C_PULLUP_INT’ treatment in the Wire.begin() call. This was somewhat contrary to the general run of the posts on the Teensy forum, so it was a bit disconcerting. However, I ran a series of experiments that clearly showed that I2C between two Teensy 3.5 processors didn’t need external pullups, and O’scope waveform analysis showed no difference between using external 2.2Kฮฉ pullups and no external pullups with the ‘I2C_PULLUP_INT’ option.

However, when I switched to the ‘Wire.h’ library, all this changed. I had to use external 2.2Kฮฉ pullups – and this was verified via O’scope analysis. This usually isn’t a big deal, but it turns out that in my case it’s going to be a real PITA to add the pullups. my hardware configuration uses all point-to-point jumpers and no PCB, so there just isn’t any easy way to do this.

You would think that, since there is obviously a way to enable the pullups on the I2C lines (obviously, because that is exactly what the i2c_t3.h library does), there must be a way to do the same thing with the Wire.h library. You would think that, but I’ll be darned if I can figure it out. I have posted this issue to the Teensy forum, but so far no luck finding a solution.

OK, I may have found a clue. Buried deep in i2c_t3.cpp is the following macro:

it is this macro that actually casts the magic spell over the currently defined SCL & SDA pins to enable (or disable) internal pullups.

29 January 2022 Update:

After a lot of forum and Google searching, I decided to try some small experiments regarding how to set an ‘open-drain’ or ‘input-pullup’ configuration on a Teensy 3.5 GPIO pin. Here’s the code:

And here’s some of the output:

In particular, I was able to indirectly measure the pin pullup resistor value by tying the pin to GND through a 10Kฮฉ resistor. The voltage at the junction was about 0.78V, so the voltage divider equation Vr2 = V *R2/(R1+R2) when solved for Vr2 = 0.78V and R2 = 10K gives R1 ~33K, which agrees well with the known pullup resistor value for the Teensy 3.5.

From this it seems that I should be able to initialize the appropriate pins for I2C with ‘wirex.begin()’ and then follow that with the code to set the pins for input_pullup. We’ll see.

I uploaded a very basic I2C ‘master’ sketch to my T3.5, as shown:

And verified with an O’scope that the SDA & SCL pins were active, and that external pullup resistors were required.

When I added ‘pinMode(SCL1, INPUT_PULLUP);’ just after Wire1.begin(), the I2C activity was disabled – no signal at all on either line. I tried some other combinations of pinMode() and digitalWrite(), but nothing changed – clearly the use of pinMode() and/or digitalWrite() overwrites at least some of the required configuration for I2C output.

So, next I plan to try some direct port control and see if that will do the trick.

From Kurt E’s spreadsheet, SCL1 & SDA1 (pins 37/38) are PortC pin 10 & 11 respectively. So,

PORT_PCR_MUX(n): selects the ALTernate function for the pin in question. PORT_PCR_MUX(1) just selects the pin as a GPIO. For instance, to select T3.5 pin 37 as I2C1 SCL, we would use PORT_PCR_MUX(2). Thus, the code line might look like:

where ‘PORT_PCR_ODE’ is the defined constant that selects the ‘Open-Drain-Enable’ bit in the Port Control Register for Port C, bit 10, which is connected to T3.5 pin 37. ‘PORT_PCR_ODE’ is defined in Kinetis.h as:

The ‘0x00000020’ part, when translated to binary is: 0000 0000 0000 0000 0000 0000 0001 0000 << selects the 5th bit in the 32-bit Port Control Register.

PORT_PCR_MUX(n) is defined in Kinetis.h as:

so PORT_PCR_MUX(2) –> (uint32_t)(((2 & 7) << 8)) –> (uint32_t)(((0010 & 0111) << 8)) –> (uint32_t)(((0010) << 8)) –> (uint32_t)(0010 << 8) –> 0000 0000 0000 0000 0000 0000 0000 0010 << 8 –> 0000 0000 0000 0000 0000 0010 0000 0000 –> 0x200

Based on the above information, I thought that I might be able to accomplish my goal by using the following construct in setup():

Unfortunately, this did not work; I2C activity on pins 37/38 ‘flatlined’ and that was that. However, after letting my mind work on the problem while the rest of me slept, I had a new thought when I woke up this morning. Maybe the problem with the above construct is the ‘PORT_PCR_MUX(1)’ fragment. Maybe selecting the default GPIO port overwrites the prior I2C function selection?

So, I decided to try again this morning, using ‘PORT_PCR_MUX(2)’ (I2C function selected) instead. The code looks like this:

And, “son of a gun” – it worked! pins 37/38 activity continued, and physical pullups aren’t required – YES!

Here’s the full program:

I2C activity visible on scope with 2.2K pullup resistors (foreground under green wire jumper) disconnected

I went ahead and connected my plug-board ‘master’ with the Teensy 3.5 on my ‘second-deck’ plate to test end-end I2C comms without external 2.2Kฮฉ pullups. I loaded the Wire library master_reader example on the plugboard Teensy 3.5, and the slave_sender example on the ‘second-deck’ Teensy 3.5. Then I connected Wire1 on the master to Wire (19/18) on the slave. I modified both the master and slave examples with the

On the affected lines (37/38 on the master, 19/18 on the slave). When I ran the examples, I got the following output:

I ran the above experiment with both ends modified for internal pullups, just one end modified, and one or both modified with external 2.2K pullups. the link worked perfectly for all these cases. Her’s a photo of the setup:

master/slave I2C connection with 6″ jumpers. Note 2.2K resistors NOT used

Frank

Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part II

2 Replies

Posted 12 December 2021,

Just after getting to the point noted at the end of Part I of this saga, I discovered that I had managed to kill the Teensy 3.5 I was using at the time. This post (and maybe subsequent ones) describes my efforts to determine what happened, and how to keep it from happening again.

Well, there was a small detour on the way to the forum…. After installing a Teensy 3.5 on the robot chassis and wiring everything up, I did something that killed the Teensy. I was connecting the charger to the robot, and noticed that the Teensy rebooted (I was running a sketch that blinked the on-board LED 5 times/sec). That should never happen, so I disconnected and reconnected the charging plug a few more times. Sometimes (but not all the time) the Teensy would reboot – and then it stopped responding entirely; the LED was still blinking, but it was much dimmer, and the Teensy would no longer respond to programming inputs. I measured the 3.3V regulated output, and it was now down to about 1.9V – ouch!

I spent a LOT of time looking around for the cause of the problem without finding anything really credible. I finally decided that having a Teensy input connected directly to the pin on the charging jack that gets disconnected from GND when the charging plug is inserted must be problematic somehow. So I added an RC filter to the line so that when the disconnect occurred, the signal presented to the Teensy pin would be filtered through the RC filter, as shown below:

This didn’t work either – a brand-new Teensy rebooted after just a few connect/disconnect cycles – rats!

Next I abandoned the idea of using the normally closed switch portion of the charging jack entirely, and decided instead to look at a stepped down (4:1 divider) version of the +12V input line with one of the Teensy’s many analog inputs, as shown below:

This too failed to protect the Teensy, and after a few connect/disconnect cycles it too was completely unresponsive; now I’m a multiple Teensy 3.5 killer – yikes!

After killing not one, but two Teensy’s, it was time to realize I simply did not know what was happening, so as usual I went back to basics. First I connected yet another expensive Teensy 3.5 to my PC via USB, but this time I simply placed it near, but not on, my robot chassis. Then I performed 10-20 charging plug connect/disconnect cycles, and confirmed that the Teensy still lived. While this step seems pretty stupid, it at least eliminated some form of magic that killed all Teensy’s within some radius of the robot chassis. Next, I placed this Teensy on the robot chassis, but not connected electrically to anything – just the same USB connection back to my PC. Another 10-20 connect/disconnect cycles with no complaints, and now I’m convinced whatever is killing Teensy’s is a conducted signal, not radiated.

Next, I connected the Teensy’s ground pin to the ground side of the 5V LDO output, which is also the ground side of the charging connector. Another 10/20 cycles with nothing bad happened, and now I’m convinced that whatever is killing Teensy’s is a conducted signal, but not something inherently on the ground line only.

At this point I realized that since I now had Teensy ground, USB ground, and robot power ground all connected together, I could now look at the +12V side of the charger power supply with my Hanmatek DOS1102 100MHz digital O’scope, and compare/contrast it to my lab power supply output when using the latter as a substitute charger input. After a bunch more connect/disconnect cycles with both my charger power supply and my lab power supply acting as a charger input, I noticed that the charger power supply exhibited very pronounced ‘ringing’ on the +12V line when repeatedly connected and disconnected from the charging jack, as shown below:

Charging PS ‘ringing’ during connect/disconnect cycles. Note scale is 5V/div

In the above photo, the vertical scale is 5V/div, meaning that some of the excursions are upwards of 15V (plus AND minus!) – plenty enough to kill a Teensy.

So now I knew what was happening, but not why. After some more playing around I realized that the really big ringing pulses were caused by just tapping the front of the charging plug against the front of the circular jack opening — and then I realized that the front surface of the charging plug isn’t insulated – it’s actually the same surface as the +12V inner cylinder!

Plug on left from Wall-E2 charging cable. Plug on right doesn’t cause problems

So, I began to understand that the why was because I was occasionally shorting the +12V output of the power supply to ground, effectively shorting out the power supply. The power supply doesn’t die because it has short-circuit protection, but the process of shorting and then opening the circuit causes (I think) the ringing.

So, why didn’t I see this before? I’ve been using this same power supply and jack for (literally) years with my older 4WD Wall-E2 robot, with no problems. The answer (I think) is shown in the following photo:

Charger connection port on Wall-E2 robot

As can be seen in the above photo, the charger jack on Wall-E2 features a lead-in ‘registration’ cone so that slightly off-center alignments can be accommodated. An unintended (and unknown till now) side-effect of this arrangement was to prevent the front face of the old charging plug from touching the grounded inner outer circumference of the jack.

Assuming all this holds water as I move forward, I can go back to looking at the normally-closed grounding switch on the charging jack for physical charger plug insertion detection, or leave it the way it is now with the 4:1 voltage divider on the +12 line to the charger module.

13 December 2021 Update:

Well, maybe not. I removed the probe lead-in collar from my old robot and put it on the new one, then re-ran the experiment where I look at the output of the 4:1 voltage divider wrt robot system ground, and I still see very large +/- excursions when I connect and disconnect the charging power supply using the original plug. So, There may be something else going on, or it may be that these perturbations have existed all the time, but the Mega2560 wasn’t negatively affected. When I do the same experiment with my lab power supply, I see almost no excursions or ‘ringing’. The next experiment will be to swap the connectors (conductor-faced one on my lab supply, insulator-faced on on my charging supply) and see what happens.

Well, not so clear; when I swapped plugs I could still see significant, but much reduced, excursions when connecting/disconnecting the charging supply. However, the lab supply looked pretty clean, with either connector. Here are the screen grabs from my scope for all four conditions

Charging PS, Old (conductor-faced) Plug
Charging PS, New (insulator-faced) Plug
Lab PS, New (insulator-faced) Plug
Lab PS, Old (conductor-faced) Plug

From the above plots, it looks like the charging PS with the new (insulator-faced) plug should be OK. We’ll see.

16 December 2021 Update:

After thinking about the situation for a while, I realized there was simply no way to avoid potentially dangerous (to a Teensy, at least) +/- voltage excursions on anything connected to the +12V charging input line, so “hoping for the best” is probably not the best plan moving forward. I started thinking about optical coupler ideas, and after sleeping on this for a couple of nights, I realized that I already had half of an optical coupler available; the TP5100 charger typically comes with a two-color (red/green) LED for visual display of charge/end-of-charge states. This LED typically isn’t installed, but the PCB pads are there and ready to go. So, If I installed the LED and placed a photodetector of some sort nearby, I could implement a non-conductive connection between the charger +12V input and my poor defenseless Teensy 3.5 – woohoo!

I poked around in my parts bin for a while, and came up with some photo transistors and a couple of small, simple GL5537 photoresistor parts. According to the datasheet, it exhibits a dark resistance in the meg-ohms and a fully-illuminated resistance in the single-digit ohms – perfect! So I whipped up a small breadboard circuit to test this out, as shown in the following photo:

GL5537 Photoresistor nose-to-nose wth the green/red dual color LED shipped with each TP5100 charger

I used a 20K resistor pullup to 3.3V to simulate a Teensy GPIO pin set for a digital input with a pullup resistor, and this worked great; with the LED OFF, the scope showed very nearly 3.3V, and nearly zero with the LED ON (I did have to cover the photoresistor/LED combination with an opaque shade to keep my lab overhead lights from interfering though). Now the +12V supply and anything electrically connected to it can perturbate all it wants to – it won’t be able to kill any more Teensys because the only electrical connection now is through the battery itself to the +5V LDO regulator and then through the 5/3.3V regulator on the Teensy – yay!!

The next step was to integrate the photoresistor and LED onto the TP5100 charging module. I was able to replace the voltage divider resistors on the perfboard holding the TP5100 charging module with the photoresistor and I installed the 2-color LED on the PCB pads provided on the TP5100 module, with the leads arranged so that the LED boresight pointed toward the photoresistor, as shown below:

With this setup, I can connect/disconnect either style charging plug to my heart’s content without damaging anything. The only question remaining is whether or not I should install a light-shield around the LED/photoresistor pair. I could do that easily with a small section of heat-shrink tubing, but I’m not sure it’s necessary; in addition, if I do that I’d lose the ability to visually confirm that the charger is actually charging. Something to think about, anyways.

15 January 2022 Update:

While the above solution worked perfectly for decoupling any charging plug connection transients from the Teensy(s), it meant that the charge status LED on the TP5100 module wasn’t very (i.e. not at all!) visible from the outside of the chassis, so I had no way to visually confirm that plugging in the charging cable was actually doing something. I thought about this for a while, and had the idea that this might be the perfect place for a ‘light pipe’. After some Google searching, I found this part.

This light pipe worked great, and there was just enough length for me to attach the input end to the TP5100 charge status LED and the output end to the front panel of the robot, as shown below:

black 4″ light pipe attached with hot glue to charge status LED
Output end of light pipe installed on Wall-E3 front panel.
Red ‘charging’ status light visible from across the room
TP5100 charge status LED changes to green to indicate full charge

Stay tuned,

Frank

Wall-E3 Replacing Mega 2560 With Teensy 3.5

2 Replies

Posted 23 October, 2021

For some time now, I have been contemplating a replacement for the outdated and obsolete Mega 2560 micro-controller as the main controller module of my autonomous wall-following robot. The robot actually sports three controllers – the Mega 2560 main controller runs the overall operating system, drives the motors, and manages the speaker, the front-facing LIDAR unit, and a number of LEDs. A Teensy 3.2 manages the forward-facing IR-LED charging station beacon detection/homing algorithm, and a Teensy 3.5 manages the seven VL53L0X LIDAR distance measuring modules. So, it would make a lot of sense to replace the 2560 with another Teensy, making all the software consistent, and giving the robot a significant boost in computing power. A Teensy 3.5 at 120MHz runs at almost 10 times the 16MHz rate of the 2560, has twice the Flash memory (512K vs 256) and 32 times the RAM (256K vs 8). However, until recently (see this post) it has been very difficult/impossible to perform over-the-air (OTA) firmware updates with any of the Teensy products – a deal-breaker for me as I do that quite often with Wall-E2. Now that OTA capability has been demonstrated, the only remaining hurdle was whether or not a Teensy 3.5 has sufficient I/O to replace the Mega 2560. To address this question I created an Excel spreadsheet to document the current I/O assignments of the 2560 and then tentatively assigned Teensy 3.5 pins to match, as shown below.

As can be seen from the above, the Teensy 3.5 has plenty of pins left over after all currently assigned Mega 2560 pin assignments have been covered, so that problem goes away too.

The last piece of the puzzle was just the inconvenience of changing out a currently-working Mega 2560 just to gain performance that may or may not really be necessary. However, now that I have decided to redo the entire chassis to address the turn side-slip problem (see this post), I plan to use the opportunity to start fresh with a Teensy 3.5 as the main controller and work out any problems with just the chassis, the motors and motor drivers and the Teensy before moving on to implementing the rest of Wall-E2’s capabilities.

Motor Drivers for Wall-E3:

I have a bunch of motor driver modules left over from an earlier motor driver study, so I decided to use two of the Pololu VNH5019 motor driver modules for Wall-E3 – one for the two left motors, and one for the two right motors.

01 November 2021 Update:

The first step in bringing up the Wall-E3 robot was to assemble a very basic system on a simulated chassis bottom plate, as shown in the following photo:

Prototype Wall-E3 System on simulated chassis baseplate

The basic system consists of a TP5100 charger system and a 8.4V 7200 mAH battery pack, an Adafruit 1NA169 high-side current sensor, a Teensy 3.5 and a auxiliary board containing a 5V LDO regulator and a HC-05 Bluetooth module. The Teensy’s program at this stage consists of the OTA update code developed earlier in this post and this post, plus a few lines to read the current sensor output, convert it to a current value, and send it to the BT serial port. Here’s the program:

And here is some of the output:

With this setup I noticed there were a lot of ‘dropouts’ in the sensor outputs, so I added a 1uF cap between the sensor Vout and GND. This seemed to cure the dropouts as shown in the next output snippet (after some charging time resulting in lower charging current)

04 November 2021 Update:

I installed two Pololu VNH5019 drivers onto the prototype bottom plate, and copied the associated motor support code from my latest Wall-E2 version. I modified ‘SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)’ and ‘SetRightMotorDirAndSpeed(bool bIsFwd, int speed)’ for the VNH5019 pinout and truth table, and that was pretty much all I had to do. Then I copied all of ‘CheckForUserInput()’ from Wall-E2 (and added the ‘U/u’ case for OTA updates) so I could control the motor direction and speed via the OTA serial connection, and the motors and drivers behaved pretty much right away. Here’s a short video showing the action:

05 November 2021 Update

I finally got the new 200 x 230 mm aluminum chassis finished, as shown in the following photo:

10 November 2021 Update:

After getting the new aluminum chassis fabricated, it was time to start populating it with subsystems from the prototype chassis and from Wall-E2. As shown in the following photo, the next big milestone was to populate it with four Pololu D-20 motors, the battery/charger and 5V LDO regulator subsystems, and the two Pololu VNH5019 motor drivers from the proto chassis. With the addition of four 3D printed wheels/tires from Wall-E2, and Wall-E3 was officially ‘mobile’!

Basic self-contained Wall-E3 system

As can be seen above, the Teensy 3.5 controller and its companion 5V LDO regulator board/HC-05 Bluetooth module were simply hot-glued to the battery pack for initial testing; they will be moved to the top plate next.

After successful movement tests with the above arrangement (see the companion post), the next step was to install the Teensy 3.5 controller and the LDO/HC-05 module on the top plate, and permanently install the charging connector and the ON/OFF switch. I decided that a single hole punched into the center rear of the top plate would suffice for bringing necessary wiring up from the internal battery/motor bay onto the top plate, and placing the hole toward the rear would allow the top plate to be removed and placed upside down at the back of the chassis to allow access to the battery/motor bay as needed. As shown in the following photo, the top plate is secured upside-down on the main chassis by two of the normal mounting screws, thus keeping the top-plate-mounted subsystems from contacting the ground.

Top plate in the ‘open’ position

Also note that the motor/battery bay is much cleaner than the Wall-E2 layout, even though both motor drivers are in this bay rather than mounted to the top plate as they are for Wall-E2. The next photos below show the chassis closed up with the Teensy 3.5 and the 5V LDO module mounted on the top plate, with the charging connector and the ON/OFF switch permanently installed.

Front & rear aspect views of the ‘closed’ chassis

After getting everything closed up, I re-ran the ‘spin turn’ trials, as shown below:

‘Spin Turn’ trials with T3.5 and 5V LDO mounted to top plate

At this point, there are only three significant modules (the MPU6050 IMU, the ‘Run Current’ 1NA169 high-side current sensor, and the charging station beacon homing module) remaining to install on the top plate. The first step will be to install and test the MPU6050, as that will give Wall-E3 the capability to do angle-controlled turns.

20 November 2021 Update:

As described in the companion post to this one, my plan is to move forward with the Teensy program (Teensy_MPU6050_DMP6_V3.ino) and the older I2CDevlib/MPU6050 library versions while waiting for Jeff Rowberg and company to figure out why the current versions of this library are no longer compatible with the Teensy 3.x line of microcontrollers.

The first step in this process is to remove the Arduino UNO/Wixel Shield module from the robot, and get the already-installed Teensy 3.5 talking to the already-installed MPU6050 module using the above mentioned Teensy_MPU6050_DMP6_V3.ino and the older I2CDevlib/MPU6050 library versions. Here’s the hardware layout:

Teensy 3.5, MPU6050, 5V LDO Regulator/HC-05 BT module, ‘I_Run’ current sensor

And here’s some output from Teensy_MPU6050_DMP6_V3.ino showing a ‘live’ connection to the MPU6050:

At this point, we have demonstrated we can talk to the MPU6050 using Jeff Rowberg’s (old) I2CDevlib library and can pull off yaw values, so we should also be able to do the same turn-rate-controlled turns as before.

So now I have a Teensy project (Teensy_MPU6050_DMP6_V3) that demonstrates a working interface to the MPU6050, a prior project (T35_WallE3_V1) that demonstrated Teensy OTA updates using an HC-05 Bluetooth module, and a Teensy project demonstrating turn-rate controlled turns using the MPU6050 (T35_MPU6050_Demo_V2). The next step is to create yet another new project, this time cleverly called ‘T35_WallE3_V2’, to merge all these features into one project, and then hopefully proceed from there.

When I created the T35_WallE3_V2 project, the first thing I did was copy in T35_WallE3_V1.ino and supporting files, and then I tested it to make sure everything was OK. However, this project wouldn’t connect to the MPU6050 even though I was sure it was using the older I2CDevlib files. After a very careful comparison, I found there were a very few differences. When I instead used the support files from my Teensy_MPU6050_DMP6_V3 project, then T35_WallE3_V2 was able to connect to the MPU6050. The i2Cdev.h/cpp files were essentially identical. The MPU6050.h/cpp files were essentially identical. The MPU6050_6Axis_MotionApps_V6_12.h files were essentially identical, so I’m not really sure why the magic happened — but it did!

So now the T35_WallE3_V2 talks to the MPU6050. Next I enabled ‘board.txt’ and verified that I can still do OTA firmware updates.

21 November 2021 (an almost palindromic date 21_11_21) Update:

After verifying OTA update capability with T35_WallE3_V2, I started porting in turn-rate control code from the Teensy_MPU6050_DMP6_V3 project, consisting mainly of the PID values empirically found in the companion ‘form-factor’ post, and the PIDCalcs() function.

  • Replaced T35_WallE3_V2 ‘CheckForUserInput()’ with version from T35_MPU6050_Demo_V2
  • Replaced T35_WallE3_V2 ‘PIDCalcs()’ with version from T35_MPU6050_Demo_V2
  • Replaced T35_WallE3_V2 ‘SpinTurn()’ with version from T35_MPU6050_Demo_V2 and added TURN_START_SPEED constant.
  • Updated TurnRate_Kp/Ki/Kd to 1.8/1.0/0.4

22 November 2021 Update:

At this point I have T35_WallE3_V2 to the point where I can do OTA firmware updates via the HC-05 Bluetooth link, and I can remotely command CW & CCW turns, forward and backward, speed up/down. Here’s the snapshot of the code at this point.

Now that I have turn-rate-based turns working properly, it’s time to turn my attention to the other peripherals – namely the ‘I_total’ and ‘I_run’ 1A169 high-side current sensors, the battery voltage output from the 5V LDO regulator module, and the charge connection status line.

  • Ported GetTotalAmps(), GetRunningAmps(), IsStillCharging(), IsChargerConnected, and MonitorChargerUntilDone() from FourWD_WallE2_V12.ino.
  • Ported CHG_SUPP_PARAMETERS into PRE-SETUP
  • Ported charger support pin init into setup()

After these ports, I got a lot of ‘mySerial was not declared’ errors – not surprising as I needed ‘mySerial’ in the Arduino world to access ‘printf’ formatting, but the Teensy has that feature built-in. After replacing 5 instances of ‘mySerial.’ with ‘Serial1.’, I recompiled – still lots of errors – oh well.

In the Arduino code, I used a function called ‘GetAverageAnalogReading()’ to average A/D readings with a ‘CURRENT_AVERAGE_NUMBER’ parameter. However, in the Teensy world there is a function called

which causes all the ADC channels to average every reading by ‘num’ (where ‘num’ is internally forced to be 0,4,8,16, or 32). So, I can eliminate the ‘GetAverageAnalogReading()’ function entirely for the T3.5, and just use analogRead().

After some minor fumbling around to comment out or bypass non-relevant (to this phase, at least) code, I got the program to compile. Now to test for proper behavior…

Testing for the two 1NA169 current sensors went OK, but I hit a snag when testing the ‘Charger Connected’ line from the charging plug. This line is tied to GND with no jack inserted, and the GND connection is broken when the plug is fully inserted. The pin is initialized for INPUT_PULLUP, meaning the line will be pulled HIGH via the internal pullup resistor when the charging cable is plugged in, and pulled to GND when the plug is removed. My initial pin assignment for this pin was 36/A17, but I discovered this wouldn’t work as some analog inputs didn’t have pullup resistors available. So, I more or less randomly chose pin 28 on the other side for this, only to find out that for no apparent reason, the pullup on pin 28 no longer seemed to exist. The charge connect line indeed measured GND with the plug removed, but only about 0.1-0.2V with the plug engaged, instead of 3.3V as it should. After some screwing around, I finally constructed a very small test program to exercise pins 28-32, whereupon I found that 29-32 operated properly, but 28 refused to behave. I posted about this to the Teensy forum, and another forum member tried my program on one of their T3.5’s and reported that pin 28 behaved properly on their module – rats! In any case, I can move the line to pin 29 no problem, but it’s frustrating to realize that of all the pins I could have picked, I managed to hit the one that misbehaved :(.

25 November 2021 Update (Happy Thanksgiving!)

I’m writing this from the home of my stepson and his family in St. Louis, MO, where we are all gathered for the traditional Turkey Day dinner. Of course I brought my Wall-E3 robot so I could continue to work on bringing it along. While here I managed to get the CHG_CONNECT issue solved, but in the process may have killed another port on the Teensy 3.5. At the moment I have the CHG_CONNECT input assigned to pin 31, and the CHG_CONNECT_LED output assigned to pin 30. Pins 28 and 32 appear to be inactive and might have been casualties in some previous project, or casualties of this one – hard to tell. In any case that all appears to be working now.

The following photo shows the current pin assignments for the T3.5. These may well change before the whole adventure ends, but it’s a good starting point.

29 November 2021 Update:

Here’s my first cut at a system-level schematic for the new Wall-E3 robot:

Wall-E3 System Schematic

The above schematic has the same basic layout as for Wall-E2, with the following changes:

  • The Wixel RF serial extender module has been replaced by the HC-05 Bluetooth module (physically located on the small perf-board module along with the 5V LDO voltage regulator)
  • The left and right motor drivers are now VNH5019 parts instead of DRV8871
  • The MPU6050 & IR Detector modules now use their own I2C channel (SDA1/SCL1) instead of everything being serviced on the single I2C channel available on the Mega 2560
  • The VL53L0X ToF sensor array is still managed by a separate Teensy 3.5, but now is the only device on the primary (SDA0/SCL0) I2C bus

At this point I have verified proper operation for both VNH5019 motor drivers, both 1NA169 inline current sensors, the MPU6050 IMU and the HC-05 Bluetooth module.

The next big milestone is to get the second-deck mounted and verify proper operation of the forward-looking LIDAR and the VL53L0X side and rear ToF distance sensors.

Stay tuned,

Frank

Another try at Wall Offset Tracking

1 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

1 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

1 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