Tag Archives: Teensy 4.1

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

Teensy 4.1 Replacement for Teensy 3.5?

Leave a reply

A while back, I had some problems with damaged Teensy 3.5 main controllers on Wall-E3, my autonomous wall-following robot. I eventually traced the problem back to large voltage transients that occurred when I connected/disconnected the charging probe. These transients were conducted to the Teensy 3.5 pin used to detect the probe connection status, causing the Teensy to immediately reboot, and then eventually become unusable. I solved this problem by using a non-conductive photonic charger connect/disconnect system using the supplied charge LED on the TP5100 charger coupled to a photoresistor.

Unfortunately, I ran through most of my Teensy 3.5 stock while I was figuring this out, and now this part is unavailable from PJRC or anywhere else – maybe a victim of the world-wide chip shortage? The good news though is that the Teensys 4.1, successor to the Teensy 3.5 is available, so I purchased a few to evaluate as a replacement for the T3.5 on my robot.

The Teensy 4.1 has the same form factor and pin layout as the T3.5, which means it is a drop-in replacement in most cases. However, there are some gotchas. According to the comparison sheet on Paul Stoffregen’s PJRC site, the T4.1 is 5X faster (600MHz vs 120MHz) and has more memory. However, it doesn’t have any analog output DACs (T3.5 has 2), and more importantly, the T4.1 pins are not 5V tolerant!

To start my evaluation, I loaded a simple ‘blink’ program, and as expected it worked great. Here’s the test setup, utilizing my newly-discovered OONO breakout board.

Teensy 4.1 on the OONO breakout board

After this first test, I am convinced that the T4.1 will work nicely as a T3.5 replacement, except possibly for the 5V tolerance issue. To investigate this, I looked at my current system schematic

Wall-E3 System Schematic

Wall-E3’s main controller is directly connected to two VNH5019 motor controllers, several LEDs (the Chg Stat Display module), two INA169 current sense modules, a Pulsed Light (now owned by Garmin) LIDAR system via it’s ‘Mode’ pin, the ‘HC-05 BT Module’ (now replaced by my new 5V Reg/Wixel board) via its TX/RX pins, and the battery pack via the ‘Chg Conn’ pin (this the photonic connection discussed above). It is also connected to a MPU6050 IMU, a Teensy 3.2 running the IR charging beam detector, and another Teensy 3.5 running the 7-element VL53L0X distance sensing array, all via two different I2C ports.

The I2C ports are no problem, as they are either Teensy-to-Teensy or Teensy-to-MPU6050, which has an onboard regulator to regulate 5V down to 3.3, so the data lanes are 3.3V. The LED panels is passive (doesn’t generate any signals of its own), so that’s not a problem. The Wixel RF Transceiver inside my 5V Regulator/Wixel module runs on 3.3V, so the RX/TX lines are compatible with Teensy 4.1. The ‘Irun’, ‘Itot’ and ‘BattV’ A/D inputs are all below 3.3V at their maximum values (The Irun and Itot lines max out at about 2V (2 amps through the current sensor), while BattV maxes out at about 2.4V (8.4V max battery voltage minus 6V drop through a 6V zener diode).

The ‘Mode’ line on the LIDAR could be an issue. The original Pulsed Light LIDAR was acquired by Garmin, so the original datasheets are no longer available. The Garmin datasheet says that the MODE pin output is limited to 3.3V, but I don’t know if that is the same for the original Pulsed Light model I’m using on Wall-E3. So, I hooked up my digital O’scope to the LIDAR’s MODE pin on Wall-E3 and measured it directly. As shown in the following scope grab, the output is indeed limited to 3.3V – yay!

Pulsed Light LIDAR-Lite MODE pin pulse output. Note max amplitude (Ma) = 3.308V

So, it looks like I can drop a Teensy 4.1 into Wall-E3’s system and it should do fine. I don’t really need the speed and/or memory improvements, but I do need a replacement for the now-defunct Teensy 3.5, in case I manage to kill yet another one.

21 March 2022 Update:

I took the time today to see how (or if) the Teensy 4.1 breakout module would fit on my current Wall-E3 robot. As shown below, it’s pretty big!

Based on the above photos, I don’t think this breakout board has a future on Wall-E3. Even though the module will fit, that doesn’t take into account the fact that the wiring from the module will extend horizontally from the module, rather than vertically as it does with the plain ‘Teensy + Female header’ arrangement. It was a great idea a the time, but I guess the reality is that the breakout module will be relegated to testing,

07 September 2024 Update:

I was playing with my 4WD robot and noticed the MPU6050 IMU wasn’t responding correctly, so to start the troubleshooting process I pulled the MPU6050 off the robot and connected it to a Teensy 4.1 instead of the Teensy 3.5 I have in the robot, because I had a 4.1 available and didn’t have a 3.5. This led to a couple of interesting discoveries:

  • I discovered it is very difficult to get a Teensy 4.1 with pins to fit into my ‘small’ plugboard. No amount of finger pressure would get the pins to fit into the correct sockets. This continued until I discovered that, way back in the day when I first got a couple of 4.1’s to try, I had soldered header pins to all the pins on the 4.1, including the five pins across the breakout board – oops! A few seconds with a side-cutter to remove these pins and I was back in business.
  • The default Wire1 pins on a Teensy 4.1 are different than the ones on a 3.5/3.2, and this threw me for a bit of a loop. Eventually I figured out that Wire1.begin() gets aimed at the proper default pins based on the compile target – Teensy 4.1 vs Teensy 3.5.

After making these changes, I was able to compile and run my ‘Teensy_MPU6050_DMP6_V4.ino’ Arduino sketch and verified that my MPU6050 module was working fine.

Stay tuned,

Frank