Tag Archives: MPU6050

Porting LIDAR-based ‘Find Best Direction’ to Vision-Enhanced Robot

Leave a reply

Posted 13 October 2026

While working on my new ‘vision-enhanced’ robot project, it became apparent the the OAK-D Lite camera was not the best tool for handling a ‘stuck’ condition where the robot has nosed into something and needs to find a better movement direction. The camera After some experimentation, I found that by moving the rear-facing VL53L1X LIDAR distance sensor to the front of the robot, I could turn the robot under manual (‘C’) control and watch the reported LIDAR distances change. Since I had already solved the ‘Find Best Direction’ problem on my older robot, I decided to port the code into the new vision-enhanced robot.

Looking back through my old code in C:\Users\Frank\Documents\Arduino\WallE3_Git\WallE3_Git\WallE3_Git.ino, I found a reference to this post. It wasn’t directly relevant as it discussed the problem of sorting multiple arrays in a synchronized fashion so as to arrive at a situation where all the distance and heading sensor arrays are sorted in lockstep with a ‘master’ array containing front distance readings. Then the ‘dependent’ array readings associated with the maximum front distance reading can be easily read off.

This isn’t really necessary for the vision-enhanced robot, as there are only two sensors of interest – the front distances and the associated headings. I believe I should be able to set up two small lists (say 10 elements; front distance readings and associated headings. As the robot rotates, each new distance reading is compared to the stored ones, and replaces the largest value that is smaller than it. The headings array value associated with the replaced heading value gets replaced by the heading value associated with the new distance value. At the end of the rotation, the distance array should contain the 10 highest distance readings, sorted in descending order, and the heading array should be synchronized to their associated distances. Then an ‘appropriate’ heading can be selected from the data (‘appropriate’ to be defined later).

It turns out UpdateIMUHdgValDeg() already manages a HEADING_HISTORY_ARRAY_SIZE array of heading values, where earlier values are bumped up (down in index) and the latest value is stored at gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE – 1]. If I also have a HEADING_HISTORY_ARRAY_SIZE array of distance values that are handled in the same manner, that should be all I need. I’m pretty sure I already have that array, as that is what is used to calculate variances needed to detect ‘stuck’ conditions. Yep – its there but named ‘aRearDistCM’ instead of ‘aFrontDistCM’. I’ll let that ride until everything is working and then change ‘Rear’ to ‘Front’ everywhere. CalcBruteRearDistArrayVariance() is also there for stuck detection. bool IsStuckBehind() is missing so copied it from WallE3_Git.ino to VisionEnhancedRobot…’. ino (into MISCELLANEOUS section).

Looking back through the code and previous posts, I built up the ‘QuickSort’ algorithms to handle just this situation, so abandoning them for something simpler does not seem real smart; it’s already there, and it works.

Well, maybe not. Looking at WallE3_Git.ino, I see that the RunToDaylight() code does not appear to use QuickSort at all – it does the ‘simpler’ idea of rotating 10 deg at a time, recording both heading and distance. After the rotation I see:

which is pretty much exactly the ‘simpler’ idea I was thinking about.

Well, I may have spoken too soon. WallE3_Git.ino has *two* ‘RunToDaylight()’ functions, ‘RunToDaylight()’ and ‘RunToDaylightV2()’, and ‘RunToDaylight()’ is apparently never called and was replaced by ‘RunToDaylightV2()’ which incorporates the QuickSort algorithms. However, only RunToDaylightV2() Step5:

applies to this situation – all the other code is aimed at finding a trackable wall. Step5 is basically ‘RunToDaylight()’ with the distance and heading arrays presorted by decreasing distance with

I made the decision to port all the ‘RunToDaylightV2()’ and supporting code to the new ‘stuck-recovery’ branch, trimmed down to just the ‘Hdg’ and ‘FrontD’ arrays for now (may later want to add RearD, but not now). This is basically just the ‘Step5’ portion of ‘RunToDaylightV2()’, with the addition of all the supporting code for QuickSort operations.

I have the ‘stuck ahead’ detection running now. Here’s a sample output:

The next step is to use the ‘RunToDaylightV2()’ function and supporting QuickSort code to determine the best direction to travel *after* IsStuck() returns TRUE.

18 October 2026 Update:

After getting the stuck-recovery code working, I created a new C++ project on my PC to parse through the output generated by the 360 degree scans from ‘stuck-recovery’. Here’s the final code:

Including both 36-point (10 deg steps) and a 72-point (5 deg steps) datasets from the ‘stuck-recovery’ code. With the 72-point (5 deg steps) scan data active, the .cpp program produces the following output.

The above plot was generated using a small Python program generated by Grok, as shown below:

19 October 2026 Update:

Grok modified the ‘polar_scan.py’ script to pull the required ‘bestStart’ and ‘bestEnd’ values directly out of the raw data from the robot. With the following data

The polar_scan.py program produced the following plot:

This is a bit of an unrealistic scenario, as I deliberately started the robot scan with its front toward the open doorway, in order to excite the edge case where the ‘best direction’ lobe crosses over the +/- 0 deg boundary. In reality the ‘best direction scan will almost certainly start with the robot pointed away from the ‘best’ direction, as that condition is what triggers the ‘run to daylight’

It is also unrealistic in another way, in that this data was collected with my robot sitting on my bench, with the robot’s VL53L1X distance sensor about 90cm off the ground. In reality it will be more like 17cm off the floor in real operation.

Here is the scan from the robot sitting on the floor, starting with its nose just a few centimeters away from a simulated wall, with the open area behind it.

As the plot shows, there is a fairly wide of headings from about -135 to +135 deg, with the peak at about -170 or so. This corresponds very to the actual layout of my lab.

I attached my iPhone to the robot in the same place as the VL53L1X sensor and ran another 360 scan. The result was this video:

261019 360 deg scan in 5-deg steps, starting with the robot’s nose just a few cm from a simulated wall

The far doorway can be seen in the video at about the 8 second mark.

20 October 2026 Update:

The ‘Find Best Direction’ study is complete. the ‘polar_scan.py’ script takes the scan data from the robot and produces the polar plot shown below:

Now it’s time to integrate all this back into the mainline repo. I had a conversation with Grok about this, and I believe I am coming around to his/its way of thinking. Have the teensy handle initial ‘stuck’ detection using a variance threshold on the front-facing VL53L1X. If a stuck condition is detected, notify the Pi5. The Pi5 tells the teensy to back up 1 sec & perform a 360 deg scan and report distances and headings. The Pi5 will then run the above ‘polar_scan’ script to determine the best escape direction and tell the teensy to turn to that heading. After that the Pi5 uses the camera to validate the escape direction, and if it agrees, normal travel/navigation is resumed.

21 October 2026 Update:

I committed/pushed the ‘stuck-recovery’ branch from my PC – this is all the stuck-recovery scan code, but minus all the required integration code.

So, what to do about the big switch statement in CheckForUserInput? Right now it switches on the first character of the string. Some commands are truly single-character (C, H, U…) but some are single-alpha + numeric (Txx, Lxx, Rxx), where the ‘xx’ are arbitrary numeric characters. Now I want to add full-string commands like ‘STUCK_SCAN’, but this results in “Unknown top-level character”. I could add a ‘S’ case, but I might eventually run out of single-character commands. Maybe the way to handle this is with a ‘!’ (bang) prefix, which says “look at the full string”. A case statement like “case ‘!’: would handle all multi-alpha characters commands and would then use internal ‘if’ statements to accommodate an arbitrary number of commands

So, I started with “!STUCK_SCAN”. When this string is entered at the serial port, CheckForUserInput() runs this code:

It stops the motors and then calls RunToDaylightV2():

which repeatedly calls SpinTurn(true, 5) to scan 360 degrees.

This all works great – except – – – when RunToDaylightV2 is running a scan, it can’t be stopped – oops! The reason it can’t be stopped is because it isn’t calling CheckForUserInput() inside the ‘for’ loop. The obvious answer is to place a call to CheckForUserInput() inside the loop and then send a ‘C’ character to the teensy, which will stop the motors ….. except that it doesn’t; it stops the motors, but then the next call to SpinTurn() starts them up again – rats!

So, next we try using the bool return from CFUI() to guard the SpinTurn() calls, like so:

But, this doesn’t work either, because the call to CFUI here doesn’t have anything to do with the part of CFUID that called RunToDaylightV2 – completely separate. So, I finally decided that the only way to reliably handle this (and other similar) issues is to establish a global ‘status’ variable ‘gl_AnomalyCode’ that can be set in CFUID and read anywhere. I actually had this in my previous WallE3_git project, and even had a separate ‘enum.h’ file to house the anomaly codes and other enums. I copied just the AnomalyCode section from enum.h, as shown below:

Then I added the global variable ‘uint8_t gl_AnomalyCode = ANOMALY_NONE;’ to hold the current anomaly code, and set it to ‘ANOMALY_STUCK_AHEAD’ in the ‘!STUCK_AHEAD’ CFUI section, and back to ‘ANOMALY_NONE’ in the ‘C’ CFUI section. This fixed the problem entirely (but not necessarily elegantly). RunToDaylightV2 checks the value of gl_AnomalyCode each time through the scan loop, and terminates if it isn’t ANOMALY_STUCK_AHEAD.

After sleeping on this, I started thinking that what I really want instead of (or maybe in addition to ANOMALY_CODES is a SYSTEM_STATUS enum, with STATUS_MANUAL_CONTROL set system-wide whenever the user inputs ‘C’.

enum SystemStatus {   STATUS_MANUAL_CONTROL = 0,   STATUS_AUTO_NAV }; SystemStatus gl_SystemStatus = SystemStatus::STATUS_AUTO_NAV;

Then, in RunToDaylightV2():

    if (gl_SystemStatus == STATUS_MANUAL_CONTROL)     {       return;     }

And in CheckForUserInput() -> ‘C’ case:

gl_SystemStatus = SystemStatus::STATUS_MANUAL_CONTROL

And in CheckForUserInput() -> ‘C’ case:

And in RunToDaylightV2():

I will keep the AnomalyCode stuff around as well, but just not for this purpose:

After making these changes, I ran the Teensy program and simulated a Pi5 command to perform a stuck scan by sending “!STUCK_SCAN’ at the PC serial port, and then sent ‘C’ after the scan actually started, with the following output:

23 October 2026 Update:

So now I have the teensy RunToDaylightV2() and CheckForUserInput() code modified to handle ‘!STUCK_AHEAD’, ‘STATUS_MANUAL_CONTROL’ and ‘!STUCK_SCAN’ situations, it’s time to make the associated changes/additions to the Pi5 code.

In a ‘stuck_ahead’ situation, the teensy sends ‘!STUCK_AHEAD’ to the Pi5 via Serial1, and it is captured by the ‘telemetry_loop()’ function.

As shown in the code above, any line from the teensy of the form “[timestamp]: [body]” where ‘body’ is “DONE [cmd]”, “ACK [cmd]”, or “BUSY [cmd]” has the [body] extracted and placed in the ‘teensy_events’ queue. This queue is examined in ‘wait_for_done’ and if one of the above strings is present, does the right thing. This isn’t directly applicable to the ‘stuck’ situation, but something similar will be necessary. ‘telemetry_loop’ will have to recognize the anomaly flag from the teensy and pass it along to the main nav function

Moving Vision Processing to My 4-Wheel Robot, Part 2

Leave a reply

Posted 03 August 2026,

Part 1 in this series laid out the strategy for adding vision processing capabilities to my 4-wheel robot. This post continues that effort.

The first step was to replace the Teensy 3.6 main controller with a Teensy 4.1. As part of this process, I also replaced the glued-on ‘Gnd’ and ‘+5V’ bussbars with a bit more finished product, as you can see in the following photos:

The next step is moving the old 4-wheel firmware to the ‘new-improved’ 4-wheel hardware. I started this by porting “C:\Users\Frank\Documents\Arduino\WallE3_Git\WallE3_Git\WallE3_Git.ino” to “C:\Users\Frank\Documents\Robot_Projects\VisionEnhaned4WheelRobot_V1\VisionEnhaned4WheelRobot_V1.ino”. This took a while, and I’m not sure I wouldn’t have been better off just cloning the entire WallE3_Git.ino file, but I “got it done’.

05 August 2026 Update:

It took a while, but I now have a working 4-wheel drive robot program running again on the 4-wheel chassis with a Teensy 4.1 main controller The only small ‘gotcha’ I encountered during the Teensy 3.5 -> Teensy 4.1 controller switch was the control lines for the right motor driver. It turns out the ‘speed’ (PWM) pin used on the Teensy 3.5 isn’t PWM-capable on the Teensy 4.1, so this required some minor pin swapping, as shown below:

Using the manual control features built accessible via the ‘CheckforUserInput() function, I was able to perform left/right 10 degree turns and speed up/slow down, all from my keyboard. I don’t have the second deck connected up yet, but that is next on the agenda.

Upgraded 4-wheel robot demonstrating basic motor

06 August 2026 Update:

4-wheel robot with Raspberry Pi5 and OAKD-Lite camera mounted on 2nd deck

At this point I have a complete 4-wheel drive robot assembled with a Raspberry Pi5 and OAKD-Lite camera, and I can send it basic (left/right/faster/slower/forward/reverse) movement commands via a serial port. In previous work on the 2-wheel robot I demonstrated the ability to send vision-processing-derived movement commands to the main processor from the Pi5 via a serial port.

It occurs to me that with the above capability, The robot doesn’t really need anything else for navigation and obstacle avoidance. The OAKD-Lite can provide visual depth information to the Pi5, and the Pi5 can send basic movement commands to the robot. In addition, it seems to me that I might be able to eliminate the entire ‘homing-to-charger’ software and hardware blocks, as the camera should be able to tell the Pi5 where the charger dock is located in the field of view, and the Pi5 should be able to tell the robot which way to turn and by how much, and when to speed up and slow down. This could make the robot a LOT simpler (as Elon Musk said about Tesla FSD “Once you have vision solved, you no longer need anything else’).

08 August 2026 Update:

The robot is alive! With Grok’s help I now have a python script running on the Pi5 that can direct the robot to turn left or right depending on image processing by the OAKD-Lite camera.

First, the Python shell script ‘restart_camera.sh’ is executed to get the OAKD-Lite camera ready:

Once the camera is ready, then the demo program ‘clearest_direction_node.py’ is executed on the Pi5 (with ‘VisionEnhanced4WheelRobot.ino already running on the Teensy 4.1)

Based on a rudimentary analysis of the depth (distance) distribution in the camera field of view, the ‘clearest_direction_node.py’ Python script sends left/right/stop commands to the Teensy 4.1 from the Pi5’s serial port to the Teensy 4.1’s ‘Serial1’ port. The commands use the already-existing robot manual motion control command structure shown below:

Here’s a short video showing the action

Here’s another short video showing the robot actually making 10-deg turns left & right in response to a simulated obstacle in the field of view:

And here is some of the telemetry output from the above run:

At this point in the project, we have demonstrated that visual ‘depth’ information can be used to guide the robot, and we have separately demonstrated that the Teensy firmware can be updated ‘Over The Air’ (OTA) via the wifi connection to the Pi5. I believe the next step should be to add wifi OTA to the ‘clearest_direction_node.py’ script in order to remove the requirement for physically attaching a USB cable to the Teensy 4.1 to update the firmware.

08 August 2026 Update:

Adding Wifi OTA to the current ‘clearest_direction_node.py’ configuration turned out to be relatively easy, as this ability had already been developed as a stand-alone demo back in June 2026. All we had to do was update the ‘board.txt’ file to point to the new location of ‘PostBuild_OTA.py’ on my PC, and make sure ‘Wifi_OTA.py’ was running on the Pi5 so it would look for changes in “/home/pi/my_vision_robot/firmware/latest.hex” and trigger the update.

09 August 2026 Update:

Thinking while drifting off to sleep last night, I realized that the addition of the Pi5 to the robot means that all the navigation and obstacle avoidance tasks (including possibly navigating to and connecting with the charger module) will be handled by the Pi5, which significantly simplifies the Teensy 4.1 code. All the Teensy code has to do now is respond to motion commands from the Pi5, monitor the various hardware sensors and update the rear-facing LED array.

Teensy Responsibilities:

  • Run the motors in response to motion commands from Pi5 on Serial1
  • Update the ‘state-of-charge’ (and left/right steering) LEDs
  • Monitor the output from three different 1NA169 current sensors (I_chg, I_tot, I_upper-deck)
  • Monitor the output from two different Vbatt voltage sensors (V_batt, V_upper-deck)
  • Monitor the ‘charger connected’ output from the TPS5100 battery pack
  • Monitor the rear-facing VL53L1X LIDAR for rear collision avoidance
  • Run the speaker
  • Home in on and connect to the charger module (may be done by the Pi5 instead)

Pi5 Responsibilities:

  • Manage all navigation and obstacle avoidance tasks
  • Possibly manage charger module homing (April tag?)
  • Create map (SLAM)?
  • Respond to rear-facing obstacle alerts from Teensy
  • Pass robot telemetry data on Serial1 to PC
  • Pass Pi5-software-related telemetry data to VS Code terminal

10 August 2026 Update:

Good progress on the vision-enhanced robot project today. Yesterday we installed two additional cases in the switch block that accepts manual control commands for the robot, to allow the user to command left/right turns of an arbitrary number of degrees at an arbitrary turn rate. The command format consists of the letter ‘L’ (left/CCW) or ‘R’ (Right/CW) followed by the number of degrees to turn (and optionally a specified turn rate in deg/sec), but the commands failed. Today (while waiting in the cellphone lot for my wife) I figured out what was wrong with the teensy command parsing code. Now I can type “L30.5” at a VS Code prompt that gets transmitted via SSH over Wifi to the Pi5, then via the dedicated serial to the Teensy. Then the Teensy calls ‘SpinTurn(30.5, rate = 30) to actually effect the turn. Here’s a short video showing a left (CCW) turn of approximately 90 deg at a 90 deg/sec rate, followed by another left turn of 90 deg at the default 30 deg/sec rate, followed by a right turn of 180 deg at the default 30 deg/sec rate:

We also implemented a replacement for my old telemetry display using the Visual Studio serial port monitor. In theory I could maintain that feature as I still have the HC-05 Bluetooth serial module installed on the robot, but I would rather put everything over wifi for better range & reliability. What we did instead was to have WallE_5.py log everything that comes to it on its serial port (Teeny ‘Serial1’) to a ‘telemetry.log’ file, which allows me to start up a new Pi5 terminal session in its own window and run

to display whatever telemetry information the teensy is sending to ‘Serial1’. Here’s a short sampling of the telemetry data setup:

15 August 2026 Update:

I discovered that the Teensy 4.1 was unresponsive – yuk! After some research and troubleshooting, I realized that I had fallen into a classsic Teensy 3.5->4.1 transition trap; Teensy 3.5 is 5V tolerant on (almost) all pins, while the 4.5 is not. I didn’t really look hard at the circuit before making the 3.5->4.1 change, but almost all the peripheral hardware produces 5V signals on the MCU pins – oops!

So, I have spent the last few days going backwards – from Teensy 4.1 to Teensy 3.5. I took the opportunity to improve wire/pin labelling while doing the reverse transition. Here is the updated schematic:

And here are some photos showing the wire labelling:

I created a small test program, originally to just test the rear-facing VL53L1X LIDAR, but later expanded to test all sensors. Here’s the code:

And here’s the output showing all three current sensors, the battery voltage, and the rear-facing VL53L1X LIDAR output (variations in the rear distanced are due to my waving my hand in front of the sensor).

I wasn’t quite sure what to make of the zero readings for Bot_I, but then I realized that when the USB cable is plugged into the Teensy 3.5, it provides all the current needed for the first deck peripherals, meaning the current through the Bot_I INA169 sensor is essentially zero.

Next I ported the MPU-6050 support code from the VisionEnhanced4WheelRobot project into my little demo program and put some code in ‘loop()’ to add the IMU heading output to the sensor line. This didn’t work at first, but with Grok’s help I traced the problem to a disconnected power/ground lead from the main power distribution bus to the MPU6050. Once this was connected, the demo program started working right away:

I also installed and tested a front-facing red laser diode and exercised the on-board speaker. At this point I believe I have exercised/tested all the sensors, the laser diode and the speaker. I haven’t run the motors with this program as I believe that will happen when I get back to the ‘mainline’ VisionEnhanced4WheelRobot.ino program.

16 August 2026 Update:

I back-ported the code from my VL53L1X_Demo program back to my VisionEnhanced4WheelRobot program and after the normal number of mistakes, got it going. Here’s a small bit of output from loop(), with me rotating the robot back and forth to show variation in the ‘Hdg’ and ‘RearCm’ values:

Then I fired up the VS Code connection to the pi5, started WallE_5.py supervisor program and verified that I could send motor commands in the WallE_5.py window and have the robot respond appropriately – yay. I also verified that ‘tail -f ./logs/telemetry.log’ in a separate VS Code terminal window produced the same telemetry output as above.

17 August 2026 Update:

I finally have the ‘VisionEnhanced4WheelRobot’ program running again with a Teensy 3.5. The last little bit was undoing the change in motor control pin assignments for the right-hand motor driver. Going from Teenys 3.5 to 4.1 I had to change the pin assignments to place the ‘speed’ pin on a PWM-able output pin, and going back I didn’t initially undo this pin assignment – all fixed now.

At this point the robot firmware is pretty much complete – it correctly monitors all three current sensors, the battery voltage, the current heading, and the rear distance reading. I think the next step is to work on the python side to add more navigation and obstacle-avoidance ‘smarts’.

Stay tuned!

Frank

Help! I’m Spinning out of Control!!

Leave a reply

I have been running ‘field’ (i.e. in my house) tests with WallE3, my autonomous wall-following robot. Unfortunately, WallE3 has demonstrated an unfortunate tendency to lose its mind and start spinning out of control – “around and around the robot goes, and when he stops, nobody knows!”. After a number of trials where this happened, I realized I’m going to have to figure out how to detect this condition so I can get WallE3 to recover properly.

Fortunately, WallE3 knows its relative heading, thanks to its onboard GY-521 MPU-6050 3 Axis Accelerometer/Gyroscope. So, I thought I should be able to detect the ‘spinning’ condition by monitoring the relative heading numbers; if the relative heading values traversed a full 360⁰ within a reasonably short period of time like 3-5 sec, then the robot should be stopped and a recovery algorithm of some sort implemented.

As usual, a seemingly simple algorithm turns out to not be quite as simple as it seems at first glance. The first thing I tried to do was to use my new robot telemetry visualization tool to go back through my recorded telemetry files to find some runs where spinning occurred. Unfortunately, I couldn’t find any – bummer! Not to worry, I decide I could use Excel to ‘invent’ a spinning event by generating a series of monotonically increasing heading values. Then I used Excel and VBA to work out an algorithm for ‘help, I’m spinning’ detection. Shown below is a screenshot of the Excel spreadsheet, and a screenshot of the VBA code that does the detection.

Now to see if this idea actually works ‘in the wild’ (or at least ‘in the robot’)

13 June 2023 Update:

I wanted to capture data from a real ‘spinning’ event to further test the above algorithm, so naturally WallE3 has refused to cooperate, even after several trial runs. So, being the sneaky person I am, I decided to add ‘#define HEADINGS_ONLY’ and associated code section to WallE3’s code base, so I can capture heading date while manually spinning the robot. This worked well, and because it is in a #define block, it gets compiled out for normal operations. After getting that working, I captured a bunch of heading data and dropped it into my Excel setup to see how my VBA code worked with ‘real’ data. As it turned out, this exposed a bug in the algorithm – I had forgotten to handle the case where the cumulative heading is negative, but with a magnitude greater than 360. The fix was to compare the absolute value to 360, as shown in the revised code below:

Stay tuned,

Frank

WallE3 Wall Tracking, Revisited

Leave a reply

Posted 12/23/22

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

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

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

WallE3_WallTrackTuning_V5:

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

Timing intervals:

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

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

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

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

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

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

MoveToDesiredFront/Back/Left/RightDistCm()

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

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

01 January 2023 Update:

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

Offset Capture with ‘RotateToParallelOrientation’ ‘at end

02 January 2023 Update:

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

Here’s an early run:

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

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

movie frame grab just after ‘RotateToParallelOrientation()’

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

Robot orientation immediately after ‘RotateToParallelOrientation()’

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

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

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

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

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

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

01/09/23 Update:

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

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

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

11 January 2023 Update:

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

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

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

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

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

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

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

Using steerval only. Note monotonically decreasing distance

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

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

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

12 January 2023 Update:

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

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

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

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

WallE3 Rolling Turn, Revisited

Leave a reply

Posted 04 December 2022,

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

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

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

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

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

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

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

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

180º turn at 90º/sec

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

05 December 2022 Update:

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

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

Stay tuned,

Frank

Wall-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

Using Wire1 & Wire2 with the I2CDevLib & MPU6050 Libraries

Leave a reply

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 

CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

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 III

Leave a reply

Posted 04 January 2022

At the conclusion of my last post on this subject, I had refurbished the charging station IR transmit subsytem with a new perfboard setup to replace the ugly terminal strip implementation. The next step in ‘bringing up Wall-E3’ is to verify IR Homing functionality. The IR transmitter is modulated by a very stable 520.8Hz square wave produced by the Teensy 3.2 waveform generator, and this signal is demodulated by a Teensy 3.2 on the robot (see this post for the details). This technique has been working for years with the Wall-E2 robot, and since I simply moved the entire IR homing subsystem from Wall-E2 to Wall-E3, I have some hopes that it will work correctly (fingers crossed!). Here’s the test setup:

The first step was just to verify that both phototransistors were receiving the IR signal, as shown in the scope photo below:

At this point I also verified that rotating the robot caused the left and right detector signals to vary appropriately, so that all works. The next step is to verify that the demodulator output still behaves properly. To do this, I ported the appropriate code from my Wall-E2 project into T35_WallE3_V4.

06 January 2022 Update:

When attempting to verify IR Demodulator output from the onboard Teensy 3.2, all I got was zeros across the board. After investigating some more, I came to the conclusion that the main Teensy 3.5 was for some reason unable to even talk to the IR Demod Teensy via I2C. After the usual amount of fumbling around and searching the Teensy forum, I began to think that the problem was caused by the lack of proper pullup resistors on the I2C bus connecting the two. In the case of Wall-E2, I had pullup resistors mounted on the Wixel shield board, but this did not get transferred over to Wall-E3. I was also misdirected a bit by the IC2_PULLUP_INT defined value for Wire.begin calls – this, at least in my mind, implied that the internal pullups available in the ARM processor would work with two Teensy processors – apparently not. So, in order to run this issue down, I created a small test bed consisting of a T3.5 ‘master’ and a T3.2 ‘slave on a plug-in board as shown below:

I2C Master/Slave Test Setup. Note 2.2K pullup resistors above ‘master’ T3.5

Then I loaded the ‘Basic_Master’ and ‘Basic_Slave’ i2c_t3 library sketches and verified that the master and slave were communicating properly. Then I removed the pullups and changed the ‘Wire.begin()’ statements to use I2C_PULLUP_INT to see if they would still communicate – and, contrary to some posts on the Teensy Forum they did! I grabbed some scope photos of the SDA & SCL lines with and without external 2.2K pullups, as follows:

With external 2.2K pullups
Without external pullups

As can be seen from the above photos, there is no perceivable difference between the waveforms for the ‘with’ and ‘without’ external pullups cases. So, for at least this very simple Teensy-to-Teensy case, pullups don’t appear to be required.

Next, I changed the configuration to replace the ‘Slave’ T3.2 with the T3.2 mounted on the IR detector housing, as shown below:

Slave T3.2 replaced by T3.2 on IR detector housing. MPU6050 not connected to I2C

Then I loaded T35_WallE3_V4 onto the ‘master’ T3.5 and re-ran the experiment. This time the T3.5 detected the T3.2 and produced valid output, as shown below:

plot of IR detector module output while moving IR xmt back and forth past detector hood

As can be seen from the above, the computed ‘IRHomingValTotalAvg’ value varies with the alignment of the IR transmitter module. Moreover, since this value is computed using the N-path bandpass filter algorithm implemented earlier, it also verifies that the code is running properly on the IR detection/homing module, as well as the corresponding code in the main controller Teensy 3.5. Yay!

While the above validates the T3.5 main controller and the I2C bus between it and the T3.2 IR Demodulator module, it doesn’t explain why it didn’t work on the actual robot. The only difference between the configuration tested above and the configuration on Wall-E4 is the presence of the MPU6050 module on the same I2C bus. So, I added this piece back into the circuit as shown below:

MPU6050 added to I2C bus

With this configuration, the main controller failed to connect to the IR demod module, just as it did on Wall-E3. So, at least the failure is consistent with previous work. To further investigate, I looked at the I2C bus waveforms with and without the MPU6050 module on the bus, as shown below:

With the MPU6050 added to the I2C bus. Note the horizontal time scale (5 uSec/div)
Without the MPU6050 added to the I2C bus. Note the horizontal time scale (5 uSec/div)

After some more head-scratching, I finally narrowed the issue down to the double-layer connector on the MPU6050 module, where I found not just one, but two bad solder connections – oops! After repairing the connections, I reran the test with the MPU6050 on the I2C bus, and this time I got the proper behavior; case closed, and it only cost me most of a day! 🙁

07 January 2022 Update:

I re-installed the IR Homing/MPU6050 subsystem on Wall-E3 and verified proper operation. Next step will be to try some homing experiments.

Stay tuned,

Frank

Teensy, MPU6050 and Rowberg’s I2CDev Library, Take Two

Leave a reply

Posted 17 November 2021,

This post describes my efforts, once again, to figure out how to get a Teensy running with Jeff Rowberg’s I2CDev library for the purpose of interfacing to a MPU6050 6-axis IMU with on-board DMP.

After some initial teething problems, I have been using the MPU6050 for years on my various autonomous wall-following robots using Arduino Mega 2560 controllers with great success. However, when I tried switching over from Arduino UNO/Mega controllers to the Teensy 3.x controller, Jeff Rowberg’s library wouldn’t compile at all. At the time I got the library to compile by replacing #include Wire.h with #include i2c_t3.h in i2cdev.h/cpp. See this post from almost two years (December 2019) ago for the gory details.

Unfortunately, two years later when I tried to change out the Mega 2560 for a Teensy 3.5 in Wall-E3 my latest autonomous wall-following robot, I ran into compile problems again – rats! Looking at the code, I saw that the maintainers had accepted my pull request to add a ‘I2CDEV_TEENSY_3X_WIRE’ identifier to I2Cdev.h to switch from <Wire.h> to <i2c_t3.h> as I had done previously, but I still couldn’t get the library to compile with a Teensy 3.x target.

So, back to basics. Fortunately, I had kept all the old versions of the required i2cdevlib files, so my two-year old project (Teensy_MPU6050_DMP6_V2.ino) still compiled and ran properly – whew. So now to figure out why it doesn’t compile with the newest version of the libraries.

So I created a new VS2019 Arduino project called Tensy_MPU6050_DMP6_V3, copied my code and other files from two years ago, and trimmed the program down to just the code required to print out MPU6050 headings every 200 mSec.

And here is a sample of the output:

This program, using the i2cdevlib files from two years ago, compiles and runs fine. The things that have changed in i2cdevlib since then. Comparing i2cdev.h as used in this program with the current i2dev.h, I see

Changelog diffs
I2CDEV_TEENSY_3X_WIRE for convenience in switching from Wire.h to i2c_t3.h
I2CDEV_TEENSY_3X_WIRE used to switch to i2c_t3.h (as opposed to manually as before)
added ‘void *wireObj=0’ parameter to end of every I2Cdev method signature

And the difference in the i2Cdev.cpp files:

‘void wireObj’ added to all i2Cdev class method implementations
All instances of ‘wire.’ are replaced by ‘useWire->’ where *useWire is defined as a TwoWire object

In the i2Cdev class methods that actually talk to the MPU6050 over the i2c bus (readBytes, readWords, writeBytes, writeWords) the new version uses a pointer to a ‘TwoWire’ object called ‘useWire’ instead of Wire. The ‘useWire’ object is defined at the top of each of these methods with the lines

but the declaration (invocation?) of the ‘TwoWire’ class is guarded by an #ifdef as shown

Declaration of ‘Wire’ as a ‘TwoWire’ object

Which means it isn’t included if ‘I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE’ is used. In my old version of I2Cdev.cpp, this didn’t matter, because all the class method declarations & definitions use the ‘Wire.’ object technique for invoking a ‘i2c_t3’ class method. However, the new code uses the ‘useWire->’ pointer technique which does require the ‘TwoWire’ declaration, only it’s not available for ‘I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE’ – ouch!

Comparing the old and new MPU6050.h files, the only changes of significance are:

  • The name of the class was changed from MPU6050 in the old file to MPU6050_Base in the new one, with ‘, void * wireObj = 0’ added as the last parameter in the constructor
  • GetCurrentFIFOPacket, setFIFOTimeout, and getFIFOTimeout methods were added
  • The entire #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 section was removed
  • void *wireObj and uint_32t fifoTimeout objects were defined

The new GetCurrentFIFOPacket method added by Homer Creutz replaces the inline method I used in my older code, and does a nicer job to boot, and the wireObj declaration makes possible changing all the method calls from ‘Wire.’ to ‘useWire_>’ style.

Comparing the old and new MPU6050.cpp files, the only changes of significance are:

  • The names for all method implementations are changed from MPU6050:: to MPU6050_Base::, with a ‘,wireObj(wireObj)’ parameter added at the end of the constructor method and most other methods as well
  • The implementation code for GetCurrentFIFOPacket, setFIFOTimeout, and getFIFOTimeout methods were added

Comparing MPU6050_6Axis_MotionApps_V6_12.h (the old version) to MPU6050_6Axis_MotionApps612.h (the new version), there were a lot of changes:

  • The implementation code was split out from the header file into a new MPU6050_6Axis_MotionApps612.cpp file
  • A new MPU6050_6Axis_MotionApps612 : public MPU6050_Base class is declared and all the dmpxxx methods are now part of this class.
  • All the hard-coded MPU6050 DMP firmware image is gone (in the cpp file?)
  • At the very bottom is the line ‘typedef MPU6050_6Axis_MotionApps612 MPU6050;’ declaring ‘MPU6050’ to be a object of type MPU6050_6Axis_MotionApps612

The new MPU6050_6Axis_MotionApps612.cpp file now contains the DMP firmware image and the definitions for all the methods declared in the .h file.

So, it looks like the whole problem with changing over from the old setup to the new one may be just the fact that the declaration (invocation?) of the ‘TwoWire’ class is guarded by an #ifdef that disables the line for ‘I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE’. So tomorrow (neeeeeeedddddd, sleeeeepppp!) I’ll try again with the new setup, with the additional condition added to the #ifdef line.

So, I created yet another VS2019 Arduino project called ‘Teensy_MPU6050_DMP6_V3’, identical in every way to ‘Teensy_MPU6050_DMP6_V2’ except this project includes MPU6050_6Axis_MotionApps612.h instead of MPU6050_6Axis_MotionApps_V6_12.h. To make things simpler, I copied all the reference files from the i2cdevlib folder into the project’s local folder, as shown below:

After editing I2Cdev.h as shown below

to utilize the i2c_t3.h version of the Wire library, I compiled it for Teensy 3.2 and got the following output:

As shown, all the compile errors reference a missing declaration for TwoWire, which I hope means that adding the ‘I2CDEV_TEENSY_3X_WIRE’ identifier to the guard around the code that declares TwoWire in i2Cdev.cpp.

I edited i2Cdev.cpp to add the ‘I2CDEV_TEENSY_3X_WIRE’ identifier to the guard around the code that declares TwoWire in I2Cdev.cpp, and edited I2CDev.h to do the same thing for the guard around the ‘class TwoWire’ declaration, and recompiled. This time the output was:

So I commented out the ‘

//extern TwoWire Wire;

line and recompiled, still getting way too many errors – time to punt.

20 November 2021 Update:

As mentioned above, I decided to punt on the idea of incorporating the newest I2CDevlib files, as I keep getting lost trying to sort out the ‘Wire/TwoWire/useWire’ mess.

However, I did discover that the new library versions work just fine with an Arduino UNO or MEGA2560 as the target. So, I created an Arduino project called ‘Arduino_MPU6050_6Axis_MotionApps612’ and used it to tune the rate-controlled turn PID for the new robot. After the requisite number of false-starts and screwups, I was able to get pretty decent 45º/sec and 90º/sec turns with:

So, at this point I have an Arduino program ( Arduino_MPU6050_6Axis_MotionApps612) targeting the Arduino UNO that works fine with the new versions of the i2cdevlib libraries, and an older Arduino program (Teensy_MPU6050_DMP6_V3) targeting the Teensy 3.x that works fine with the 2-year old versions of the i2cdevlib libraries. The next step I think is to move ahead with the Teensy-targeted program using old libraries on the assumption that eventually Jeff Rowberg and company will either fix the current libraries to work properly with Teensy, or show me the error of my ways so that I can fix the Teensy compilation problem.

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