Vision-Enhanced Navigation | Paynter's Palace

Posted 28 June 2026

When I work on significant projects like the vision-enhanced robot project, I generally hold two different but related mental maps for the overall project.

One map describes the physical and/or logical entities that are needed for the overall project to succeed, such as wheel odometers, battery packs, power regulation/distribution, the OAKD-Lite camera, the pi5, wheel motors and drivers, etc.

The second mental map describes the software (here the term ‘software’ includes both the pi5 software and related teensy firmware) pieces needed to give the robot the ability to do what we want, primarily the enhanced navigation possible with vision processing via the OAKD-Lite camera and associated software. This mental map (at least for me) seems to be project oriented, where each project addresses different, mostly independent software capability implementations (Wifi_OTA, with its precursor projects ‘SerialPassthroughDemo’, Wifi_OTA_Demo, etc).

I tend to work on a big project like this from the top (system-view) and bottom (small sub-projects that will be later integrated into the overall project) at the same time. I have learned over the years that creating (and later modifying as needed) a clear top-down system architecture is absolutely crucial to improving the chances of getting someplace that looks like where you wanted to go. This systems architecture is the ‘aspirational view’ (to borrow a modern Elon-ism) of the project’s long-term goal. In the case of the vision-enhanced robot (VER), the goal is similar to the one for my 4-wheel robot, i.e. “Autonomous navigation around our home”. However, instead of ‘wall-following’ as the 4-wheel robot did, this project will utilize vision processing at the primary navigation technology.

Over the last few weeks, Grok Code and I have been working at the bottom of the systems architecture with things like getting OTA updates for Teensy firmware working, both via the PC -> Bluetooth -> HC-05 -> Teensy Serial channel and most recently, via the PC -> Wifi -> Pi5 -> Teensy Serial channel. After (mostly) getting the Wifi_OTA capability going, I decided it was time to stop and make sure we had a Git Repo structure consistent with the top-down view. After the requisite amount of fumbling around, we (me and Grok Code) came up with the following structure.

my_vision_robot/
├── .gitignore
├── README.md
├── docs/ ← Overall project documentation
│
├── hardware/ ← Schematics, BOMs, mechanical (future)
│
├── shared/ ← Code used by multiple projects
│ ├── firmware/ ← Shared Teensy code/libraries
│ └── software/ ← Shared Pi5 Python modules
│
├── software/
│ ├── SerialPassthroughDemo/
│ │ ├── README.md
│ │ ├── pi5/
│ │ ├── teensy/
│ │ └── docs/
│ │
│ ├── Wifi_OTA/
│ │ ├── Wifi_OTA.py
│ │ ├── README.md
│ │ ├── pi5/ (if needed later)
│ │ ├── teensy/
│ │ └── docs/
│ │
│ └── Vision_Navigation/ ← Future main vision project
│ ├── pi5/
│ ├── teensy/
│ └── docs/
│
├── tests/ ← System/integration tests
└── tools/ ← General one-off utilities

Now the challenge is to move all my sub-project files from our current quite-messy repo structure into this one. Standby!

07 July 2026 Update: Wrapping up the Wifi_OTA Demo project

This little project had more than its share of bumps and bruises, but I think Grok Code and I have finally got it done. The project has four major parts; three on my PC and one on the pi5.

The Wifi_OTA Visual Studio/Visual Micro project on my PC
A ‘board.txt’ file in the same folder as the Wifi_OTA project. This file tells VS what to do after the build step
A small Python script that copies the .HEX output from the compile to the ‘latest.hex’ file on the pi5
A Python program on the pi5 that watches for updates to ‘latest.hex’ and when one is detected, passes that along the the teensy on its Serial1 port

Here is the Wifi_OTA.ino file: All it does is blink the built-in LED a couple of times and then waits for a ‘U’ character to start the flash update process.

// Generated on: 2026-06-17 14:30:00 UTC
// Wifi_OTA.ino - UART OTA Test via Pi5 Serial1

#include "FXUtil.h"
extern "C"
{
#include "FlashTxx.h"
}

//#define USING_HC05

//needed for compile-time switching
char c;
uint32_t buffer_addr, buffer_size;

#define BLINK_TIME_MSEC 200

void setup()
{
  Serial.begin(115200);      // USB debug port
  delay(2000);
  Serial.println("\nTo USB: === Wifi_OTA - Teensy Ready ===");
  Serial.println("To USB: Send 'U' from TeraTerm on Serial2 or Pi5 on Serial1 to trigger OTA update");

#ifdef USING_HC05
  Serial2.begin(115200);     // UART to HC05
  delay(2000);
  Serial2.println("\nTo TeraTerm: === Wifi_OTA - Teensy Ready ===");
  Serial2.println("To TeraTerm: Send 'U' from TeraTerm on Serial2 to trigger OTA update");
#else
  Serial1.begin(115200);     // UART to Pi5
  Serial1.println("\nTo Pi5: === Wifi_OTA - Teensy Ready ===");
  Serial1.println("To Pi5: Send 'U' from Pi5 on Serial1 to trigger OTA update");
  Serial1.flush();

#endif
  pinMode(LED_BUILTIN, OUTPUT);

  //07/02/26 revised for more definitive successful update indication
  digitalWrite(LED_BUILTIN, HIGH);
  delay(BLINK_TIME_MSEC);

  digitalWrite(LED_BUILTIN, LOW);
  delay(BLINK_TIME_MSEC);

  digitalWrite(LED_BUILTIN, HIGH);
  delay(BLINK_TIME_MSEC);

  digitalWrite(LED_BUILTIN, LOW);
  delay(BLINK_TIME_MSEC);

  digitalWrite(LED_BUILTIN, HIGH);
  delay(BLINK_TIME_MSEC);
}

void loop()
{
#ifdef USING_HC05

  if (Serial2.available())
  {
    c = Serial2.read();
    Serial2.printf("Received %c on Serial2: '\n", c);

    if (c == 'U' || c == 'u')
    {
      Serial2.println("\nStart Program Update - Send new HEX file!");
      digitalWrite(LED_BUILTIN, LOW);   // visual feedback
      delay(500);

      //uint32_t buffer_addr, buffer_size;
      if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
      {
        Serial2.println("Failed to init buffer");
        digitalWrite(LED_BUILTIN, HIGH);
        return;
      }

      Serial2.println("Calling update_firmware() on Serial2...");
      while (Serial2.available()) Serial2.read(); // clear buffer
      update_firmware(&Serial2, &Serial2, buffer_addr, buffer_size);
    }
#else
  if (Serial1.available())
  {
    c = Serial1.read();
    Serial1.printf("Received %c on Serial1: '\n", c);
    Serial1.print(c);
  }
  if (c == 'U' || c == 'u')
  {
    Serial.println("\nStart Program Update - Send new HEX file!");
    digitalWrite(LED_BUILTIN, LOW);   // visual feedback
    delay(500);

    //uint32_t buffer_addr, buffer_size;
    if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
    {
      Serial.println("Failed to init buffer");
      digitalWrite(LED_BUILTIN, HIGH);
      return;
    }

    while (Serial1.available()) Serial1.read(); // clear buffer

    Serial.println("Calling update_firmware() on Serial1...");
    update_firmware(&Serial1, &Serial1, buffer_addr, buffer_size);
#endif

    //firmware_buffer_free(buffer_addr, buffer_size);
#ifdef USING_HC05
    Serial2.println("after update_firmware");
    firmware_buffer_free(buffer_addr, buffer_size);
    Serial2.println("Firmware update call completed. About to REBOOT...");
#else
    Serial.println("update_firmware() returned - freeing buffer");
    firmware_buffer_free(buffer_addr, buffer_size);
    Serial.println("Firmware update call completed. About to REBOOT...");
#endif
    delay(2000);  // Give time for the message to be sent

    REBOOT;
  }
  }

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

// Generated on: 2026-06-17 14:30:00 UTC

// Wifi_OTA.ino - UART OTA Test via Pi5 Serial1

#include "FXUtil.h"

extern "C"

{

#include "FlashTxx.h"

}

//#define USING_HC05

//needed for compile-time switching

char c;

uint32_t buffer_addr, buffer_size;

#define BLINK_TIME_MSEC 200

void setup()

{

Serial.begin(115200); // USB debug port

delay(2000);

Serial.println("\nTo USB: === Wifi_OTA - Teensy Ready ===");

Serial.println("To USB: Send 'U' from TeraTerm on Serial2 or Pi5 on Serial1 to trigger OTA update");

#ifdef USING_HC05

Serial2.begin(115200); // UART to HC05

delay(2000);

Serial2.println("\nTo TeraTerm: === Wifi_OTA - Teensy Ready ===");

Serial2.println("To TeraTerm: Send 'U' from TeraTerm on Serial2 to trigger OTA update");

#else

Serial1.begin(115200); // UART to Pi5

Serial1.println("\nTo Pi5: === Wifi_OTA - Teensy Ready ===");

Serial1.println("To Pi5: Send 'U' from Pi5 on Serial1 to trigger OTA update");

Serial1.flush();

#endif

pinMode(LED_BUILTIN, OUTPUT);

//07/02/26 revised for more definitive successful update indication

digitalWrite(LED_BUILTIN, HIGH);

delay(BLINK_TIME_MSEC);

digitalWrite(LED_BUILTIN, LOW);

delay(BLINK_TIME_MSEC);

digitalWrite(LED_BUILTIN, HIGH);

delay(BLINK_TIME_MSEC);

digitalWrite(LED_BUILTIN, LOW);

delay(BLINK_TIME_MSEC);

digitalWrite(LED_BUILTIN, HIGH);

delay(BLINK_TIME_MSEC);

}

void loop()

{

#ifdef USING_HC05

if (Serial2.available())

{

c = Serial2.read();

Serial2.printf("Received %c on Serial2: '\n", c);

if (c == 'U' || c == 'u')

{

Serial2.println("\nStart Program Update - Send new HEX file!");

digitalWrite(LED_BUILTIN, LOW); // visual feedback

delay(500);

//uint32_t buffer_addr, buffer_size;

if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)

{

Serial2.println("Failed to init buffer");

digitalWrite(LED_BUILTIN, HIGH);

return;

}

Serial2.println("Calling update_firmware() on Serial2...");

while (Serial2.available()) Serial2.read(); // clear buffer

update_firmware(&Serial2, &Serial2, buffer_addr, buffer_size);

}

#else

if (Serial1.available())

{

c = Serial1.read();

Serial1.printf("Received %c on Serial1: '\n", c);

Serial1.print(c);

}

if (c == 'U' || c == 'u')

{

Serial.println("\nStart Program Update - Send new HEX file!");

digitalWrite(LED_BUILTIN, LOW); // visual feedback

delay(500);

//uint32_t buffer_addr, buffer_size;

if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)

{

Serial.println("Failed to init buffer");

digitalWrite(LED_BUILTIN, HIGH);

return;

}

while (Serial1.available()) Serial1.read(); // clear buffer

Serial.println("Calling update_firmware() on Serial1...");

update_firmware(&Serial1, &Serial1, buffer_addr, buffer_size);

#endif

//firmware_buffer_free(buffer_addr, buffer_size);

#ifdef USING_HC05

Serial2.println("after update_firmware");

firmware_buffer_free(buffer_addr, buffer_size);

Serial2.println("Firmware update call completed. About to REBOOT...");

#else

Serial.println("update_firmware() returned - freeing buffer");

firmware_buffer_free(buffer_addr, buffer_size);

Serial.println("Firmware update call completed. About to REBOOT...");

#endif

delay(2000); // Give time for the message to be sent

REBOOT;

}

Here is the ‘board.txt’ file that calls the PostBuild.py program when an F5 debug compile is completed:

#Updated 06/27/26 Post-Build: Copy .hex to Pi5
recipe.hooks.deploy.preupload.1.pattern=python "C:\Users\Frank\Documents\Robot_Projects\my_vision_robot\software\Wifi_OTA\teensy\Wifi_OTA\PostBuild_OTA.py" "{vm.runtime.build.final_output_path}" "{build.project_name}" "{build.project_path}" "{vm.runtime.build.intermediate_output_path}"
recipe.hooks.deploy.preupload.1.use_shell_execute=true

#Updated 06/27/26 Post-Build: Copy .hex to Pi5

recipe.hooks.deploy.preupload.1.pattern=python "C:\Users\Frank\Documents\Robot_Projects\my_vision_robot\software\Wifi_OTA\teensy\Wifi_OTA\PostBuild_OTA.py" "{vm.runtime.build.final_output_path}" "{build.project_name}" "{build.project_path}" "{vm.runtime.build.intermediate_output_path}"

recipe.hooks.deploy.preupload.1.use_shell_execute=true

And here is the Python script that copies the .HEX output from the compiler over to ‘latest.hex’ on the pi5:

#!/usr/bin/env python3
"""
Post-Build OTA Helper for Visual Micro
"""

import sys
import os
import subprocess
from datetime import datetime

# ============== CONFIGURATION ==============
VERBOSE = False         # Set to True only when debugging
# ===========================================

def main():
    print("\n" + "="*60)
    print("Post-Build OTA Transfer to Pi5")
    print("="*60)

    if VERBOSE:
        print("\n=== Visual Micro Path Variables ===")
        print(f"build.project_path              = {sys.argv[3] if len(sys.argv) > 3 else 'Not provided'}")
        print(f"vm.runtime.build.intermediate_output_path = {sys.argv[4] if len(sys.argv) > 4 else 'Not provided'}")
        print(f"vm.runtime.build.final_output_path = {sys.argv[1] if len(sys.argv) > 1 else 'Not provided'}")
        print("="*40)

    if len(sys.argv) < 4:
        print("ERROR: Not enough arguments provided.")
        input("Press Enter to close...")
        sys.exit(1)

    intermediate_path = sys.argv[4].rstrip('\\')
    project_name = sys.argv[2] if len(sys.argv) > 2 else "Unknown"
    #hex_path = f"{intermediate_path}\\Wifi_OTA_Demo.hex"
    hex_path = f"{intermediate_path}\\Wifi_OTA.hex"

    if VERBOSE:
        print("\n=== Derived File Paths ===")
        print(f"Project Name     : {project_name}")
        print(f"Intermediate Dir : {intermediate_path}")
        print(f"Hex File Path    : {hex_path}")
        print("="*40)
        print(f"Time : {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")

    if not os.path.exists(hex_path):
        print(f"ERROR: Hex file not found: {hex_path}")
        input("Press Enter to close...")
        sys.exit(1)

    print("\nCopying .hex to Pi5...")
    
    try:
        #subprocess.run(["scp", hex_path, "pi@RobotPi5:~/my_vision_robot/firmware/latest.hex"], 
        #               capture_output=True, text=True, check=True)
        subprocess.run(["scp", hex_path, "pi@RobotPi5:~/my_vision_robot/latest.hex"], 
                       capture_output=True, text=True, check=True)

        print("SUCCESS: .hex file copied to Pi5.")
    except subprocess.CalledProcessError:
        print("\nERROR: Failed to copy .hex file to Pi5.")
        if VERBOSE:
            print("Check that the Pi5 is reachable and scp is set up.")
        sys.exit(1)
    except FileNotFoundError:
        print("\nERROR: 'scp' command not found. Is OpenSSH installed?")
        sys.exit(1)

    print(f"\nPost-build completed at {datetime.now().strftime('%H:%M:%S')}")
    print("Ready for next build.\n")

    input("Press Enter to close this window...")

if __name__ == "__main__":
    main()

#!/usr/bin/env python3

"""

Post-Build OTA Helper for Visual Micro

"""

import sys

import os

import subprocess

from datetime import datetime

# ============== CONFIGURATION ==============

VERBOSE = False # Set to True only when debugging

# ===========================================

def main():

print("\n" + "="*60)

print("Post-Build OTA Transfer to Pi5")

print("="*60)

if VERBOSE:

print("\n=== Visual Micro Path Variables ===")

print(f"build.project_path = {sys.argv[3] if len(sys.argv) > 3 else 'Not provided'}")

print(f"vm.runtime.build.intermediate_output_path = {sys.argv[4] if len(sys.argv) > 4 else 'Not provided'}")

print(f"vm.runtime.build.final_output_path = {sys.argv[1] if len(sys.argv) > 1 else 'Not provided'}")

print("="*40)

if len(sys.argv) < 4:

print("ERROR: Not enough arguments provided.")

input("Press Enter to close...")

sys.exit(1)

intermediate_path = sys.argv[4].rstrip('\\')

project_name = sys.argv[2] if len(sys.argv) > 2 else "Unknown"

#hex_path = f"{intermediate_path}\\Wifi_OTA_Demo.hex"

hex_path = f"{intermediate_path}\\Wifi_OTA.hex"

if VERBOSE:

print("\n=== Derived File Paths ===")

print(f"Project Name : {project_name}")

print(f"Intermediate Dir : {intermediate_path}")

print(f"Hex File Path : {hex_path}")

print("="*40)

print(f"Time : {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")

if not os.path.exists(hex_path):

print(f"ERROR: Hex file not found: {hex_path}")

input("Press Enter to close...")

sys.exit(1)

print("\nCopying .hex to Pi5...")

try:

#subprocess.run(["scp", hex_path, "pi@RobotPi5:~/my_vision_robot/firmware/latest.hex"],

# capture_output=True, text=True, check=True)

subprocess.run(["scp", hex_path, "pi@RobotPi5:~/my_vision_robot/latest.hex"],

capture_output=True, text=True, check=True)

print("SUCCESS: .hex file copied to Pi5.")

except subprocess.CalledProcessError:

print("\nERROR: Failed to copy .hex file to Pi5.")

if VERBOSE:

print("Check that the Pi5 is reachable and scp is set up.")

sys.exit(1)

except FileNotFoundError:

print("\nERROR: 'scp' command not found. Is OpenSSH installed?")

sys.exit(1)

print(f"\nPost-build completed at {datetime.now().strftime('%H:%M:%S')}")

print("Ready for next build.\n")

input("Press Enter to close this window...")

if __name__ == "__main__":

main()

Here’s a short video showing the Wifi_OTA update process. The video starts just after I pressed F5 to start the debug compile on my PC. After about two seconds, the built-in LED on the Teensy 4.1 (lower-left foreground) goes OFF when the HEX file transfer starts. The file transfer takes about 30 sec, and then a few seconds after the file transfer finishes, the built-in LED on the Teensy 4.1 blinks twice and then stays ON, confirming that the update was successful.

A significant part of this little sub-project was getting the Git repository set up and running, on my PC, on the pi5, and on GitHub. The same folder structure is used in all three locations, but the pi5 side only updates entries in the ‘pi5’ subfolders, and the PC side only updates the Teensy firmware. When I do an update either on the PC or the pi5, I follow the same steps each time:

git pull origin main <<— synchs the local repo with the master on Github
git add -A <<– adds anything new to the local repo
git commit -m “put my update description here”
git push origin main <<– synchs the Github repo with the updated local repo.

At this point I believe I have the basic infrastructure in place to proceed with the real project of adding vision-processing-based navigation capabilities to the robot
The OAKD-Lite camera is installed and confirmed working
The pi5 is installed and connected to the OAKD-Lite camera and via Serial1 to the Teensy4.1.
Teensy firmware can be updated via the new Wifi_OTA update channel
The Hall-effect wheel encoders are installed and confirmed working.
The next big step is to integrate the ROS (Robot Operating System) with the OAKD-Lite camera and the Teensy.

Stay Tuned!

Paynter's Palace

Tag Archives: Vision-Enhanced Navigation

New Current Sense & Step-Down Regulator Board

30 July 2026 Update:

2 Wheel Robot with Vision Processing, Part III

07 July 2026 Update: Wrapping up the Wifi_OTA Demo project