Tag Archives: IR Homing

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

Wall-E3 Charging Station Integration, Part II

Leave a reply

Posted 28 May 2022

After getting the IR homing capability working with Wall-E3, I started working on the ability to recognize the charging beacon during normal wall-following operations, and then transitioning (or not, depending on battery level) to the charge station docking procedure.

The ‘transition-to-charge’ feature assumes that the robot is tracking the right-hand wall within a few meters of the charging station, and detects the beacon. To connect to the charger, the robot must first navigate away from the wall to a point on the IR beacon centerline, turn to line up with the beam, and then follow it to the charger.

As I was testing this feature, I noticed at one point the robot detected the IR beacon while tracking the wall opposite the charger, not the wall where the charger was installed. This is a real problem, because my current ‘transition-to-charger’ algorithm assumes the geometry associated with the common wall case. In the common wall case, the robot makes a 90 deg turn away from the wall and moves to a spot calculated to put it on the beam centerline, but this won’t work at all if the robot is starting from a point on the opposite wall. In that case, the robot needs to travel forward or backward parallel to the opposite wall by a distance calculated to put it on centerline. So, the first thing the robot needs to be able to do is to detect which case (common wall or opposite wall) it is dealing with.

The primary difference between the two cases is how much the robot has to turn away/toward the wall it is currently tracking in order to point directly at the beacon generator; if it is on the common wall, then it may not need to turn at all, or may even have to turn a bit toward the wall to point directly at the generator. If it is currently tracking the opposite wall however, it will have to turn 30-60 deg away from the wall. So, the determination as to which case is in play, the robot needs to measure the number of degrees of heading change required to face the charger; if the heading change is 30-60 degrees, then it is the opposite wall case. If it is just a few degrees, then it is the common wall case.

The following short video shows the two cases and the heading change required to point to the charger in each case.

Turn to beacon performance, for opposite and common wall cases

21 June 2022 Update:

Progress has been a bit slow over the last month, as I’ve been doing other things, including recovering from an eye/scalp injury sustained at the Senior Games in Florida (those old guys can still pack a wallop!). However, I now think I have the charging station homing operation working, at least for the right-wall-tracking (the most common and probably only) case.

As it turns out, I didn’t really need to worry about the problem of the robot picking up the IR homing beacon while tracking the far wall (i.e. the wall perpendicular to the one associated with the charging station). As I noted above, the robot can sometimes ‘see’ a beacon signal above the threshold, but when it does, the steering signal (goes from -1 to +1) is always off scale on one side or another. So, by adding the requirement that the steering signal be within the range of -0.8 to +0.8, this case is entirely eliminated, leaving only the ‘same-wall’ homing case – yay!

I made a number of improvements to the IR homing code, in particular the ‘initial approach’ code that places the robot optimally with respect to the IR beacon beam, so that the subsequent ‘home-to-charging-station’ procedure is almost always successful.

In the following short video, the robot tracks the opposite wall, makes the turn to track the adjacent wall, and then detects the IR homing beacon at the conclusion of the offset capture phase for the adjacent wall. Then the robot transitions to ‘I’m hungry – feed me’ mode.

In ‘feed me’ mode, the robot first aligns itself with the beacon in a two-stage (coarse and fine) procedure. Once this is accomplished, the robot determines its distance from the beacon, and then uses this information to determine how far off the adjacent wall it should be to perfectly line up with the center of the IR beam, makes a turn to be perpendicular to the adjacent wall, and then moves forward or backward to achieve the desired offset measurement. Once this is accomplished the robot re-aligns itself with the beacon again, using the same two-stage procedure. Once all this is done, the robot then homes on the beacon to connect to the charger.

The above algorithm is shown in action in the following short video. When viewing the video, keep the following in mind:

  • The homing beacon is only recognized at the conclusion of the wall offset capture maneuver for the adjacent wall, even though the IR beacon signal is above the threshold as soon as the robot make the turn to the adjacent wall. This is done to place the robot at a known location to start the process.
  • While aligning itself to the beacon, the robot turns ON its red laser pointer to allow us humans to follow the action. Although the ‘red’ laser looks mostly white in the video, it is indeed red.
  • The beacon alignment takes place in two stages – coarse, then fine. This can be see by the speed at which the red laser dot moves back and forth.
  • In this particular video, the robot is already at the proper wall offset, so the ‘move to desired rear distance’ operation is truncated – the robot makes the turn to be perpendicular to the wall, determines it is at the proper offset, and so immediately turns back to align to the beacon signal.
Successful ‘track and then home to charger’ run

Wall-E3 Charging Station Integration

Leave a reply

Posted 14 April 2022

Wall-E3 has a significantly different form-factor than Wall-E2, requiring modification of the lead-in rails on the charging station, as shown below.

Once the required mods were made, I uncommented ‘#define IR_HOMING_ONLY in my code to have Wall-E3 concentrate solely on homing to the charging station, and then worked my way into a set of PID values (100,0,30) that gave reasonable homing performance, as shown in the following short video:

And here’s an Excel plot showing typical homing performance.

24 April 2022 Update:

The above data and video was collected using the ‘NoPing’ version of the IR Homing algorithm, so I went back and did this again using the version that monitors the front distance, both for detecting a ‘stuck’ condition and for the ability to navigate around the charging station if a charge isn’t needed. Of course, this didn’t work right away for various reasons, but eventually I got it working and made another test run. Here’s a short slo-mo video, and the accompanying Excel chart showing homing performance.

IR Homing run in slo-mo with tracking status shown on rear panel LEDs
Steering value and front distance vs time

01 May 2022 Update:

Wall-E3 has made some significant progress in the last week, and is now capable of switching from right-wall tracking to IR Homing to the charging station, as shown in the following short video and telemetry output:

automatic switching from right-wall tracking to charging station homing

Stay tuned,

Frank

Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part IV

Leave a reply

Posted 14 January 2022

After finding and fixing the connector problem that prevented the IR Homing Teensy 3.2 module from communicating via I2C with the Teensy 3.5 main controller, I was ready to move on to some real IR homing tests.

For this effort, I decided to change the IR homing algorithm over from the PID library to my home-grown PID routine, discussed here. In the process, I realized that my PIDCalcs() function was overly complex; in particular the ‘sampletime’ calling argument is never used inside the function, as a regular interval is assumed. In all my PID implementations, this is a valid assumption, as I use an ‘elapsedMillisec’ object to enforce regular calls to PIDCalcs(). After eliminating this parameter, the PIDCalcs() function now looks like this:

And the calling routine for this round of testing (not using distance information) looks like this:

The ‘steering value’ output from the IR Homing beacon demodulator ranges from -1 to +1, and motor speeds range from 0 to 255 with a median value of 127. So, I started testing WALL-E3 with a PID of 100,0,0 – the idea being that a max output of +/- 1 from the demodulator would produce an output of +/- 100, which, when used to modulate motor speeds from the median value of 127 would produce motor speeds of 227/27 in one direction and 27/227 in the other. In other words, a Kp value of 100 should produce significant motor speed modulation without hitting the motor speed limits in either direction.

I set up a small test range in my office, and made a couple of runs. Here’s a short video showing on of the runs, and an Excel plot showing steering value and motor speeds vs time.

Homing beacon on the carpet patch. Note smooth turning behavior at start due to new wheel geometry

The next step is to increase the Kp value to the point where oscillations occur, as the starting point for the Zeigler-Nichols PID tuning method. For PID = (200,0,0) I got this behavior.

And for PID = (250,0,0),

And Kp = 300

Looks like Kp = 300 might be a good place to start. So, according to the Zeigler-Nichols PID tuning method, I should use Kp = 0.5Kc = 150, Ki = 0.45Kc = 135, Kd = 0.6Kc = 180. Using these values, I get:

Needless to say, this is NOT what I had in mind! Clearly I erred somewhere along the way. Abandoning the Z-N tuning algorithm, I turned to the ‘manual’ tuning procedure I have used in the past (also discussed in the Z-N article): With Ki, Kd set to 0, increase Kp until the system oscillates, but before it becomes unstable. Then increase Ki until the oscillations stop. Then increase D until “the system achieves an acceptable quick loop to its set-point” (whatever that means).

Starting with a Kd of 200, I increased Ki in increments of 50 to get:

This looks pretty good, even as a first try. However, when compared to the original PID = (200,0,0) plot (shown below), it looks like the Ki value of 50 didn’t actually do very much one way or the other.

Increasing Ki to 100 produces the following plot.

Not much, if any, improvement. Keeping Kp = 200 and Ki = 100, try increasing Kd in increments of 25. The first increment (Kd = 25) produced a significant change, as shown below:

Which leads me to believe the 25 increment is way too much. Trying 10, we get:

PID = 200,100,10 is much nicer than the Kd = 25 version, but there was an annoying little ‘jag’ off-course right at the end – don’t know why. Just for grins I tried PID = (150,100,10):

And this looks pretty good, actually. I think I may run with these values for a while and see how it works. Here’s the video of the above run:

One final note before leaving this subject. I added some code to IRHomeToChgStnNoPings() to measure the time required to run through each motor speed determination loop. The code simply turns ON a digital output at the start, and turns it OFF again at the end. The loop runs every 200 mSec (the current value of MSEC_PER_IR_HOMING_ADJ), but the time required to do everything is only about 1.5 mSec. In other words, the Teensy sits idle for about 99% of the time while homing to the charging station. Or to put it another way, I could run the loop 10 times faster (20mSec vs 200mSec) and the Teensy wouldn’t even break a sweat!

Stay tuned,

Frank