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.

06 August 2026 Update:

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!

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