Author Archives: paynterf

Teensy Firmware OTA Update Via SSH to Pi5

Posted 21 June 2026

After getting OTA via Bluetooth to the onboard HC-05 going for the 2-wheel robot, I had an epiphany; For vision processing the 2-wheel robot uses an on-board Raspberry Pi5 with a Wi-Fi connection to my local network (and thence to my PC), so why use the HC-05 link at all? The Wi-Fi connection is much more robust than the BT/HC-05 link and is available throughout the house. The pi5 has a serial port, so in theory I could simply write a small python script to pipe characters back and forth between its Wi-Fi port and its serial port, just as the HC-05 does between its BT port and its serial port. How hard could it be?

Grok Code and I have been working on this issue for a while now and have gotten to the point where we can transfer the .HEX file from the pi5 to the Teensy once, but not multiple times. This indicates that the firmware update did not happen correctly. Also, we haven’t yet figured out how to automatically transfer the HEX file resulting from a Visual Studio compile to the pi5 so that it can be passed to the Teensy via the pi5’s serial port so we are bypassing this step by using SCP (or a copy/paste using VS Code) to create a duplicate of the HEX file on the pi5; then all the pi5 script has to do is pass lines from the local HEX file to the Teensy via serial.

Grok Code and I have been trying to troubleshoot this problem, and we don’t seem to be getting anywhere. Grok does not really know how to troubleshoot in an organized manner – it is more of a ‘random walk’ process. This post is intended to document my own troubleshooting efforts.

First, what is the basic problem? The basic problem is that multiple transfers of a HEX firmware file to the Teensy using the established BT/HC-05/Serial2 succeed, but the same process using the Wi-Fi/pi5/Serial1 link appears to succeed the first time but fails on the second attempt. Since the firmware HEX files in the two cases are identical, the problem must be somewhere in the pi5 script, either in the way lines are read from the local HEX file or in the way lines are transferred to the Teensy.

A basic assumption in the above is that the HEX file transferred to the Teensy via BT/HC-05 and the HEX file transferred to the Teensy via Wi-Fi/Pi5 are identical, so I decided to start there. Are they really identical?

Compiled firmware on VS, copy/pasted (using VS Code) from “C:\Users\Frank\Documents\Arduino\Wifi_OTA_Demo\obj\x64\Debug\Wifi_OTA_Demo.hex” to “/home/pi/my_vision_robot/tests/Wifi_OTA_Demo/Wifi_OTA_Demo.hex”. Then I copy/pasted from the pi5 file to notepad++ and compared with the original – they matched perfectly.
I modified FxUtil.cpp’s update_firmware() to add the line “out->println(line);” then updated Teensy firmware using USB connector to establish ‘known-good’ baseline. Then used pi5 script to transfer its local copy of the firmware to the Teensy, logging the transfer via VS serial monitor. The file as logged going into the Teensy and the source file on the pi5 also match perfectly. This pretty much eliminates a corrupted file transfer as the source of the problem.
Then I performed the same procedure except using the BT/HC-05 channel instead of the Wifi/Pi5 channel.

Here’s the Wifi_OTA_Demo.ino file used to run the above tests:

// Generated on: 2026-06-17 14:30:00 UTC
// Wifi_OTA_Demo.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;

void setup()
{
  Serial.begin(115200);      // USB debug

#ifdef USING_HC05
  Serial2.begin(115200);     // UART to HC05
  Serial2.println("\n=== Wifi_OTA_Demo - UART OTA Ready ===");
  Serial2.println("Send 'U' from TeraTerm on Serial2 to trigger OTA update");
#else
  Serial1.begin(115200);     // UART to Pi5
  //Serial1.println("\n=== Wifi_OTA_Demo - UART OTA Ready ===");
  //Serial1.println("Send 'U' from Pi5 on Serial1 to trigger OTA update");
#endif

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

}

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);
    update_firmware(&Serial1, &Serial, buffer_addr, buffer_size);

    //firmware_buffer_free(buffer_addr, buffer_size);
    //Serial.println("Firmware update call completed. About to REBOOT...");
    //delay(1000);  // Give time for the message to be sent
#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("after update_firmware");
    firmware_buffer_free(buffer_addr, buffer_size);
    Serial.println("Firmware update call completed. About to REBOOT...");
#endif
    delay(1000);  // Give time for the message to be sent

      REBOOT;
    }
  }

100

101

102

103

104

105

106

107

108

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

// Wifi_OTA_Demo.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;

void setup()

{

Serial.begin(115200); // USB debug

#ifdef USING_HC05

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

Serial2.println("\n=== Wifi_OTA_Demo - UART OTA Ready ===");

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

#else

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

//Serial1.println("\n=== Wifi_OTA_Demo - UART OTA Ready ===");

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

#endif

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(LED_BUILTIN, HIGH);

}

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);

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

//firmware_buffer_free(buffer_addr, buffer_size);

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

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

#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("after update_firmware");

firmware_buffer_free(buffer_addr, buffer_size);

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

#endif

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

REBOOT;

}

25 June 2026 Update:

Grok Code and finally managed to get the Teensy OTA update via Pi5/Serial1 working. Here is the final Python script on the Pi5:

#!/usr/bin/env python3
"""
Wifi_OTA_Demo.py - Final version for ongoing vision-enhanced robot project
Last updated: 2026-06-24
"""

import serial
import time
import sys
import os
from datetime import datetime

def ota_upload(hex_file_path):
    if not os.path.exists(hex_file_path):
        print(f"Error: Hex file not found: {hex_file_path}")
        return False

    print(f"\n=== OTA Upload Started at {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} ===")
    print(f"File: {hex_file_path}\n")

    ser = serial.Serial(port='/dev/ttyAMA0', baudrate=115200, timeout=3)

    try:
        print("Sending 'U' trigger...")
        ser.write(b'U')
        time.sleep(1.0)

        print("Waiting for Teensy prompt...")
        start = time.time()
        while time.time() - start < 10:
            if ser.in_waiting:
                line = ser.readline().decode('utf-8', errors='replace').strip()
                print(f"Teensy: {line}")
                if "reading hex lines" in line.lower():
                    print("✓ Got prompt")
                    break
            time.sleep(0.2)
        else:
            print("✗ Timed out waiting for prompt")
            return False

        # Send file
        print("Sending .hex file...")
        line_count = 0
        with open(hex_file_path, 'r') as f:
            for line_count, line in enumerate(f, 1):
                ser.write(line.encode('utf-8'))
                if line_count % 200 == 0:      # Fixed: was line_num
                    time.sleep(0.005)

        print(f"Sent {line_count} lines. Sending EOF...")
        ser.write(b':00000001FF\r\n')
        ser.flush()
        time.sleep(1.0)

        # Line count confirmation
        print("Waiting for line count prompt...")
        start = time.time()
        while time.time() - start < 15:
            if ser.in_waiting:
                line = ser.readline().decode('utf-8', errors='replace').strip()
                print(f"Teensy: {line}")
                if "enter" in line.lower() and "flash" in line.lower():
                    parts = line.split()
                    if len(parts) > 1 and parts[1].isdigit():
                        num = parts[1]
                        print(f"Sending line count: {num}")
                        ser.write((num + '\r\n').encode('utf-8'))
                        ser.flush()
                        break
            time.sleep(0.3)

        print("\n✅ OTA UPDATE SUCCESSFUL!")
        print("   Flash process started.")
        print("   Waiting for Teensy reboot and Serial1 re-initialization...\n")
        
        time.sleep(12)                    # Important for reliable reboot
        print("Upload process finished. You can now check Serial1 output.\n")
        return True

    except Exception as e:
        print(f"Error during OTA: {e}")
        return False
    finally:
        ser.close()

if __name__ == "__main__":
    if len(sys.argv) < 2:
        print("Usage: python3 Wifi_OTA_Demo.py <full_path_to_.hex>")
        sys.exit(1)
    
    ota_upload(sys.argv[1])

#!/usr/bin/env python3

"""

Wifi_OTA_Demo.py - Final version for ongoing vision-enhanced robot project

Last updated: 2026-06-24

"""

import serial

import time

import sys

import os

from datetime import datetime

def ota_upload(hex_file_path):

if not os.path.exists(hex_file_path):

print(f"Error: Hex file not found: {hex_file_path}")

return False

print(f"\n=== OTA Upload Started at {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} ===")

print(f"File: {hex_file_path}\n")

ser = serial.Serial(port='/dev/ttyAMA0', baudrate=115200, timeout=3)

try:

print("Sending 'U' trigger...")

ser.write(b'U')

time.sleep(1.0)

print("Waiting for Teensy prompt...")

start = time.time()

while time.time() - start < 10:

if ser.in_waiting:

line = ser.readline().decode('utf-8', errors='replace').strip()

print(f"Teensy: {line}")

if "reading hex lines" in line.lower():

print("✓ Got prompt")

break

time.sleep(0.2)

else:

print("✗ Timed out waiting for prompt")

return False

# Send file

print("Sending .hex file...")

line_count = 0

with open(hex_file_path, 'r') as f:

for line_count, line in enumerate(f, 1):

ser.write(line.encode('utf-8'))

if line_count % 200 == 0: # Fixed: was line_num

time.sleep(0.005)

print(f"Sent {line_count} lines. Sending EOF...")

ser.write(b':00000001FF\r\n')

ser.flush()

time.sleep(1.0)

# Line count confirmation

print("Waiting for line count prompt...")

start = time.time()

while time.time() - start < 15:

if ser.in_waiting:

line = ser.readline().decode('utf-8', errors='replace').strip()

print(f"Teensy: {line}")

if "enter" in line.lower() and "flash" in line.lower():

parts = line.split()

if len(parts) > 1 and parts[1].isdigit():

num = parts[1]

print(f"Sending line count: {num}")

ser.write((num + '\r\n').encode('utf-8'))

ser.flush()

break

time.sleep(0.3)

print("\n✅ OTA UPDATE SUCCESSFUL!")

print(" Flash process started.")

print(" Waiting for Teensy reboot and Serial1 re-initialization...\n")

time.sleep(12) # Important for reliable reboot

print("Upload process finished. You can now check Serial1 output.\n")

return True

except Exception as e:

print(f"Error during OTA: {e}")

return False

finally:

ser.close()

if __name__ == "__main__":

if len(sys.argv) < 2:

print("Usage: python3 Wifi_OTA_Demo.py <full_path_to_.hex>")

sys.exit(1)

ota_upload(sys.argv[1])

And here is the Teensy sketch used for the test:

To verify that the OTA update code performed properly, I ran the update three times in a row. The first time replaces whatever sketch was on the Teensy with my test sketch (that also includes the code that supports further updates), and the second and third updates confirm that the first (and the second) updates contained the crucial update support code.

At this point the ‘Wifi_OTA_Demo’ project is pretty much finished. The only step remaining is to fully automate the process.

27 June 2026 Update:

Now I have the update process updated to the point where I can press ‘F5’ in Visual Studio in an OTA-equipped Teensy project and have the resulting .hex file automatically copied over to the pi5. Here are the relevant files:

board.txt:

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

recipe.hooks.deploy.preupload.1.pattern=python "C:\Users\Frank\Documents\Robot_Projects\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

recipe.hooks.deploy.preupload.1.pattern=python "C:\Users\Frank\Documents\Robot_Projects\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

PostBuild_OTA.py:

#!/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)
        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)

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()

The next step is to put these two steps (copy the .hex file to the pi5, and then launch the update code that xfers the .hex file to the Teensy) together into one seamless automated process.

08 August 2026 Update:

I recently got my 4-wheel robot going again and moved the Raspberry Pi5/OAKD-Lite over to it from the 2-wheel robot, and ported my WallE3_Git.ino program to it as well. Due to some changes made to file locations on both my PC and on the Pi5, some changes to the OTA-related files were necessary as well. Here are the updated files:

Board.txt (lives in the same folder as the project & .ino file):

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

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

C:\Users\Frank\Documents\Robot_Projects\my_vision_robot\software\Wifi_OTA\teensy\Wifi_OTA\PostBuild_OTA.py:

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

Copies the newly built .hex file to the Pi5 so the wifi-ota-watcher
can flash it to the Teensy over serial.

Standardized destination (Aug 2026):
  pi@RobotPi5:~/my_vision_robot/firmware/latest.hex
"""

import sys
import os
import subprocess
from datetime import datetime

# ============== CONFIGURATION ==============
VERBOSE = False         # Set to True only when debugging
PI_USER_HOST = "pi@RobotPi5"
PI_DEST = "~/my_vision_robot/firmware/latest.hex"
# ===========================================


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.")
        print("Expected: final_output_path  project_name  project_path  intermediate_output_path")
        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}\\{project_name}.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(f"\nSource : {hex_path}")
    print(f"Dest   : {PI_USER_HOST}:{PI_DEST}")
    print("\nCopying .hex to Pi5...")

    try:
        subprocess.run(
            ["scp", hex_path, f"{PI_USER_HOST}:{PI_DEST}"],
            capture_output=True, text=True, check=True
        )
        print("SUCCESS: .hex file copied to Pi5.")
    except subprocess.CalledProcessError as e:
        print("\nERROR: Failed to copy .hex file to Pi5.")
        if VERBOSE:
            print(e.stderr if e.stderr else str(e))
            print("Check that the Pi5 is reachable and scp / SSH keys are 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

Copies the newly built .hex file to the Pi5 so the wifi-ota-watcher

can flash it to the Teensy over serial.

Standardized destination (Aug 2026):

pi@RobotPi5:~/my_vision_robot/firmware/latest.hex

"""

import sys

import os

import subprocess

from datetime import datetime

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

VERBOSE = False # Set to True only when debugging

PI_USER_HOST = "pi@RobotPi5"

PI_DEST = "~/my_vision_robot/firmware/latest.hex"

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

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.")

print("Expected: final_output_path project_name project_path intermediate_output_path")

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}\\{project_name}.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(f"\nSource : {hex_path}")

print(f"Dest : {PI_USER_HOST}:{PI_DEST}")

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

try:

subprocess.run(

["scp", hex_path, f"{PI_USER_HOST}:{PI_DEST}"],

capture_output=True, text=True, check=True

)

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

except subprocess.CalledProcessError as e:

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

if VERBOSE:

print(e.stderr if e.stderr else str(e))

print("Check that the Pi5 is reachable and scp / SSH keys are 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()

C:\Users\Frank\Documents\Robot_Projects\my_vision_robot\software\Wifi_OTA\pi5\Wifi_OTA.py:

#!/usr/bin/env python3
"""
Wifi_OTA.py - Automatic OTA uploader for Teensy 4.1
Watches for latest.hex and triggers OTA via Serial1

Standardized path (Aug 2026):
  /home/pi/my_vision_robot/firmware/latest.hex
"""

import serial
import time
import os
import sys
from datetime import datetime
import signal

# Configuration
HEX_FILE_PATH = "/home/pi/my_vision_robot/firmware/latest.hex"
POLL_INTERVAL = 1.0          # seconds
MIN_HEX_SIZE = 50000         # bytes
MAX_AGE_SECONDS = 300        # 5 minutes
SERIAL_PORT = "/dev/ttyAMA0"
BAUD = 115200


def log(msg):
    timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    print(f"[{timestamp}] {msg}")


def is_fresh_hex_file(path):
    if not os.path.exists(path):
        return False
    try:
        size = os.path.getsize(path)
        if size < MIN_HEX_SIZE:
            log(f"File too small ({size} bytes)")
            return False

        mtime = os.path.getmtime(path)
        age = time.time() - mtime
        if age > MAX_AGE_SECONDS:
            return False

        return True
    except Exception as e:
        log(f"Error checking file: {e}")
        return False


def ota_upload(hex_file_path):
    if not os.path.exists(hex_file_path):
        log(f"Error: Hex file not found: {hex_file_path}")
        return False

    log(f"\n=== OTA Upload Started at {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} ===")
    log(f"File: {hex_file_path}\n")

    try:
        ser = serial.Serial(port=SERIAL_PORT, baudrate=BAUD, timeout=3)
    except serial.SerialException as e:
        log(f"Cannot open {SERIAL_PORT}: {e}")
        log("Is clearest_direction_node (or another process) holding the port?")
        log("Stop the vision node first, then the watcher will retry on the next cycle.")
        return False

    try:
        log("Sending 'U' trigger...")
        ser.write(b'U')
        time.sleep(1.0)

        log("Waiting for Teensy prompt...")
        start = time.time()
        while time.time() - start < 10:
            if ser.in_waiting:
                line = ser.readline().decode('utf-8', errors='replace').strip()
                log(f"Teensy: {line}")
                if "reading hex lines" in line.lower():
                    log("✓ Got prompt")
                    break
            time.sleep(0.2)
        else:
            log("✗ Timed out waiting for prompt")
            return False

        # Send file
        log("Sending .hex file...")
        line_count = 0
        with open(hex_file_path, 'r') as f:
            for line_count, line in enumerate(f, 1):
                ser.write(line.encode('utf-8'))
                if line_count % 200 == 0:
                    time.sleep(0.005)

        log(f"Sent {line_count} lines. Sending EOF...")
        ser.write(b':00000001FF\r\n')
        ser.flush()
        time.sleep(1.0)

        # Line count confirmation
        log("Waiting for line count prompt...")
        start = time.time()
        while time.time() - start < 15:
            if ser.in_waiting:
                line = ser.readline().decode('utf-8', errors='replace').strip()
                log(f"Teensy: {line}")
                if "enter" in line.lower() and "flash" in line.lower():
                    parts = line.split()
                    if len(parts) > 1 and parts[1].isdigit():
                        num = parts[1]
                        log(f"Sending line count: {num}")
                        ser.write((num + '\r\n').encode('utf-8'))
                        ser.flush()
                        break
            time.sleep(0.3)

        log("\n✅ OTA UPDATE SUCCESSFUL!")
        log("   Flash process started.")
        log("   Waiting for Teensy reboot...\n")

        time.sleep(12)
        log("Upload process finished.\n")
        return True

    except Exception as e:
        log(f"Error during OTA: {e}")
        return False
    finally:
        ser.close()


def main():
    log("Wifi_OTA watcher started")
    log(f"Watching: {HEX_FILE_PATH}")
    last_mtime = 0.0

    if os.path.exists(HEX_FILE_PATH):
        last_mtime = os.path.getmtime(HEX_FILE_PATH)
        log(f"Initial latest.hex found (size: {os.path.getsize(HEX_FILE_PATH)} bytes)")

    while True:
        try:
            if is_fresh_hex_file(HEX_FILE_PATH):
                current_mtime = os.path.getmtime(HEX_FILE_PATH)
                if current_mtime > last_mtime + 0.5:   # small tolerance
                    log("=== New latest.hex detected! Starting OTA ===")
                    last_mtime = current_mtime
                    ota_upload(HEX_FILE_PATH)
        except Exception as e:
            log(f"Watcher error: {e}")

        time.sleep(POLL_INTERVAL)


if __name__ == "__main__":
    signal.signal(signal.SIGTERM, lambda s, f: sys.exit(0))
    main()

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

#!/usr/bin/env python3

"""

Wifi_OTA.py - Automatic OTA uploader for Teensy 4.1

Watches for latest.hex and triggers OTA via Serial1

Standardized path (Aug 2026):

/home/pi/my_vision_robot/firmware/latest.hex

"""

import serial

import time

import os

import sys

from datetime import datetime

import signal

# Configuration

HEX_FILE_PATH = "/home/pi/my_vision_robot/firmware/latest.hex"

POLL_INTERVAL = 1.0 # seconds

MIN_HEX_SIZE = 50000 # bytes

MAX_AGE_SECONDS = 300 # 5 minutes

SERIAL_PORT = "/dev/ttyAMA0"

BAUD = 115200

def log(msg):

timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S")

print(f"[{timestamp}] {msg}")

def is_fresh_hex_file(path):

if not os.path.exists(path):

return False

try:

size = os.path.getsize(path)

if size < MIN_HEX_SIZE:

log(f"File too small ({size} bytes)")

return False

mtime = os.path.getmtime(path)

age = time.time() - mtime

if age > MAX_AGE_SECONDS:

return False

return True

except Exception as e:

log(f"Error checking file: {e}")

return False

def ota_upload(hex_file_path):

if not os.path.exists(hex_file_path):

log(f"Error: Hex file not found: {hex_file_path}")

return False

log(f"\n=== OTA Upload Started at {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} ===")

log(f"File: {hex_file_path}\n")

try:

ser = serial.Serial(port=SERIAL_PORT, baudrate=BAUD, timeout=3)

except serial.SerialException as e:

log(f"Cannot open {SERIAL_PORT}: {e}")

log("Is clearest_direction_node (or another process) holding the port?")

log("Stop the vision node first, then the watcher will retry on the next cycle.")

return False

try:

log("Sending 'U' trigger...")

ser.write(b'U')

time.sleep(1.0)

log("Waiting for Teensy prompt...")

start = time.time()

while time.time() - start < 10:

if ser.in_waiting:

line = ser.readline().decode('utf-8', errors='replace').strip()

log(f"Teensy: {line}")

if "reading hex lines" in line.lower():

log("✓ Got prompt")

break

time.sleep(0.2)

else:

log("✗ Timed out waiting for prompt")

return False

# Send file

log("Sending .hex file...")

line_count = 0

with open(hex_file_path, 'r') as f:

for line_count, line in enumerate(f, 1):

ser.write(line.encode('utf-8'))

if line_count % 200 == 0:

time.sleep(0.005)

log(f"Sent {line_count} lines. Sending EOF...")

ser.write(b':00000001FF\r\n')

ser.flush()

time.sleep(1.0)

# Line count confirmation

log("Waiting for line count prompt...")

start = time.time()

while time.time() - start < 15:

if ser.in_waiting:

line = ser.readline().decode('utf-8', errors='replace').strip()

log(f"Teensy: {line}")

if "enter" in line.lower() and "flash" in line.lower():

parts = line.split()

if len(parts) > 1 and parts[1].isdigit():

num = parts[1]

log(f"Sending line count: {num}")

ser.write((num + '\r\n').encode('utf-8'))

ser.flush()

break

time.sleep(0.3)

log("\n✅ OTA UPDATE SUCCESSFUL!")

log(" Flash process started.")

log(" Waiting for Teensy reboot...\n")

time.sleep(12)

log("Upload process finished.\n")

return True

except Exception as e:

log(f"Error during OTA: {e}")

return False

finally:

ser.close()

def main():

log("Wifi_OTA watcher started")

log(f"Watching: {HEX_FILE_PATH}")

last_mtime = 0.0

if os.path.exists(HEX_FILE_PATH):

last_mtime = os.path.getmtime(HEX_FILE_PATH)

log(f"Initial latest.hex found (size: {os.path.getsize(HEX_FILE_PATH)} bytes)")

while True:

try:

if is_fresh_hex_file(HEX_FILE_PATH):

current_mtime = os.path.getmtime(HEX_FILE_PATH)

if current_mtime > last_mtime + 0.5: # small tolerance

log("=== New latest.hex detected! Starting OTA ===")

last_mtime = current_mtime

ota_upload(HEX_FILE_PATH)

except Exception as e:

log(f"Watcher error: {e}")

time.sleep(POLL_INTERVAL)

if __name__ == "__main__":

signal.signal(signal.SIGTERM, lambda s, f: sys.exit(0))

main()

These files are also saved on my Github site in the my_vision_robot repo.

These updates went very smoothly, all things considered. The only minor hitch in the get-along was getting the new .hex file copied over from my PC to the Pi5 via SCP without having to enter the Pi5 password. Here’s the relevant material from Grok:

The password prompt

That’s expected if SSH key authentication isn’t set up (or the key isn’t loaded). scp is falling back to password auth, which blocks a fully automatic F5 experience.

Fix (one-time setup on the PC):

Make sure you have an SSH key pair:

Bash

# in PowerShell or Git Bash

ls ~/.ssh/id_rsa.pub

# or

ls ~/.ssh/id_ed25519.pub

Copy the public key to the Pi (if not already done):

Bash

ssh-copy-id pi@RobotPi5

(or manually append the .pub contents to ~/.ssh/authorized_keys on the Pi)

Test that it works without a password:

Bash

scp some_small_file pi@RobotPi5:~/my_vision_robot/firmware/test.txt

After that, the post-build scp should run silently. (this still failed):

No SSH key exists yet. Generate one (ed25519 is preferred):

In PowerShell:

PowerShell

ssh-keygen -t ed25519 -C “frank@vision-robot”

Press Enter to accept the default file location (C:\Users\Frank\.ssh\id_ed25519)
You can set a passphrase or leave it empty (empty = fully automatic, no prompts)

Then copy the public key to the Pi:

PowerShell

type $env:USERPROFILE\.ssh\id_ed25519.pub | ssh pi@RobotPi5 “mkdir -p ~/.ssh && cat >> ~/.ssh/authorized_keys”

(It will ask for the Pi password one last time.)

After that, test:

PowerShell

scp $env:USERPROFILE\.ssh\id_ed25519.pub pi@RobotPi5:~/test_key.txt

If that works without asking for a password, F5 will also be password-free.

2 Wheel Robot with Vision Processing, Part II

Leave a reply

This is the second of (hopefully) many posts on my project to add modern vision processing to my autonomous wall-following robot. The first post is here.

Lots of changes since my first post. I discovered that my 8.4-to-5V LDO regulator board wouldn’t reliably drive the Raspberry Pi5, so I had Grok look around for other options. He found a step-down converter module at Pololu advertising 85+% efficiency and a much cleaner output. I have it on order so we’ll see.

I also struggled to get the Luxonis OAK-D Lite stereo camera working on my Pi5/Ubuntu 24.04LTS setup. Previously we had gotten it to work with the Pi5 running the Raspberry OS, but getting it to work with the Ubuntu OS was a lot more challenging. This effort also ran afoul of Grok’s complete inability to realize that it is in a hole and to stop digging. We went through dozens of Python scripts designed to get the camera to connect and show some data (I was connecting to the RPi5 via SSH from my windows box, so showing images wasn’t possible), and they all failed due to one subtle problem or another. After several days of getting nowhere I finally called a halt, took a couple of days off, and came back determined to start over from first principals. Instead of using Grok, I started from scratch with some web searches to find other successful implementations of The OAK-D Lite camera. At the Luxonis ‘Documentation’ site I found this page describing a viewer for the OAK-D Lite (and other) cameras. The OAK Viewer is available for windows and *nix OS, so I decided to start by trying to get images from the camera using the Windows version to bypass all the annoyances associated with peripheral handling in Linux. This turned out to be pretty much plug-and-play, and immediately the OAK-D camera showed up in the list of available devices. When I clicked on ‘Connect’ instead of connecting immediately the app immediately started measuring the available bandwidth of the USB connection as shown below.

After several USB connect/disconnect cycles, I got this display:

Oops! I hadn’t even considered the USB cable/connector bandwidth issue – and neither had Grok. For convenience I had plugged the camera cable into my USB hub, which is definitely not ‘super speed’ whatever the heck that is. After some cable and connector switching, I found that a heavy-duty Type-C cable connected directly to a Type-C connector on my Dell XP15-9530 laptop allowed the bandwidth check to succeed, and now I got some images showing up on my Windows 11 display – yay!

OAK-D Lite images. Depth pseudo-color on left, raw RGB image on right

So the moral of this story is – the Grok path was never going to work because Grok never considered that cable/usb connector bandwidth might be an issue. By going back to ‘first principals’ and taking the simplest possible path to a working camera/display configuration with a known-good Windows 11 app, I was able to immediately identify a completely unknown (to me and to Grok) – but fatal – stumbling block – USB cable/connector bandwidth. Grok has no sense of time, so every iteration was just like a puppy chasing a dog – willing to chase that ball an infinite number of times without ever thinking about the fact that ‘chasing the ball’ and ‘progress toward the goal’ aren’t necessarily the same thing. It took a mere mortal like me to say “whoa – this isn’t getting us anywhere – maybe a different approach?”

Now that I had demonstrated that the OAK-D Lite camera and the proper cable/connector combination worked – at least on Windows 11, I had a ‘known-good baseline’ that I could always retreat to, I started working on getting the OAK-D Lite camera working on the RPi5/Ubuntu camera with the same OAK Viewer application (but in the Linux flavor).

This turned out to be another maze to navigate. The Luxonis site has detailed instructions for the Linux version of the viewer, but it involves installing from a *.deb package, which unfortunately is targeted at the amd64 64-bit chip ecology – but the RPi5 uses arm64 – a different animal entirely. When I tried to install the ‘viewer.deb’ package, I got the following errors:

The following packages have unmet dependencies: oak-viewer:amd64 : Depends: libgtk-3-0:amd64 but it is not installable Depends: libnotify4:amd64 but it is not installable Depends: libnss3:amd64 but it is not installable Depends: libatspi2.0-0:amd64 but it is not installable Depends: libdrm2:amd64 but it is not installable Depends: libgbm1:amd64 but it is not installable Depends: libxcb-dri3-0:amd64 but it is not installable Recommends: pulseaudio:amd64 or libasound2:amd64 but it is not installable

It was at this point that I re-engaged Grok and started to make real progress. Grok immediately identified the Pi5/Ubuntu-compatible DepthAI Python library as the way to go and guided me through the installation process. Fortunately, this had a happy ending, even though there were several ‘gotchas’ along the road. However, since I knew for a fact that the hardware (and USB cable) were ‘known good’ elements due to my Windows 11 work, I was pretty sure any detours were software-only. After working my way through the various twists and turns with Grok’s help, we got to here – success!

Initial images captured by the OAK-D Lite camera running on my RPi5 with the Linux Ubuntu OS

Getting from my easy Windows 11 camera demo to the RPi5/Ubuntu camera demo would have been improbable if not impossible for me to do without Grok’s help. I might have gotten there, but it would have involved days/weeks of web searches and forum posts at the very least. I believe this is where Grok really shines – a definite problem with a definite end, with very few (none in my case) outside corrupting factors like the USB bandwidth/cable issue.

Interestingly, after getting the Pi5/Ubuntu/OAK-D Lite combination working, I asked Grok to help me find an easier way to take screen shots on the Pi5, and Grok obliged by offering the ‘Flameshot’ app as a substitute for the built-in Gnome keystroke shortcuts. And then we went down another rabbit-hole, and I ended up wasting an hour or so trying to get Flameshot and Gnome to work and play well together, only to wind up removing Flameshot and learning how to better use the Gnome built-in shortcuts.

So Grok is definitely a mixed blessing, and I cannot imagine how a younger less-experienced engineer would do without the (literally) lifetime’s worth of experience I have in troubleshooting hardware/software systems. When I was that young less-experienced engineer half a century ago I was trying to troubleshoot a RF EMI problem with a small electronics device made by Motorola. Eventually my supervisor suggested that I travel to Motorola and work with their engineers to figure out the problem. I did, and over the space of two days a very experienced Motorola engineer taught me the ‘divide and conquer’ method of troubleshooting that I use to this day. When Grok inevitably goes down a rabbit-hole with this young engineer in tow, who’s going to be there to throw them a life-line?

A Flagpole for the Hobbit House

Leave a reply

Posted 09 April 2026

Last month I decided it was time to put up a real flagpole to display the American flag in all its glory. Being an engineer, I had to research the heck out of the subject to make sure we got something we would be proud to have in our front yard. A neighbor has a flagpole and he had gotten a flag pole kit from Admiral Flags, right here in Columbus Ohio. I also asked Grok, and he (It?) came up with the same company, so we ordered a 20′ kit from them.

Reading through the instructions on Admiral’s website, I was amazed at the amount of concrete needed for the flagpole foundation. They called for 6-8 80lb bags of quick setting concrete (480 – 640lbs!) for an 18″ diameter by 29″ deep hole – yikes! Being a septuagenarian, I wasn’t very eager to manage 80lb bags, so I decided to start with 10ea 50lb bags (500lbs) and add more if needed. The Admiral website also strongly recommended calling Ohio811 ‘call before you dig’ utility mapping service, so I did that right away. As it turned out that was one of my better moves, as my first intended location was right on top of a utility run to the house. Dodged the bullet on that one!

My planned location in our front yard is at least 150′ away from any electrical outlet and the nearest water faucet, so I had some logistics issues to work through. I needed an additional 100′ water hose, and I decided to use my backup electric generator for electric power. Here’s a short video showing the distances.

From watching some videos regarding quick setting concrete I decided to go with the 5-gallon bucket and electric drill mixing paddle route, so I loaded my brand-new Toyota Tundra pickup with 500lbs of QuickCrete, 2ea Lowes 5-gallon buckets, a cheap 100′ water hose and the mixing paddle.

I had conned a neighbor into helping, so we started digging mid-morning, and had a hole pretty much done in a couple of hours. I was amazed that the first 6-10″ was pretty rocky, but after that it was pretty easy going. Here are some photos of the hole:

Finished hole before pouring any concrete

Hole about 3/4 full, using 50lb QuickCrete bags and some rods to keep flagpole sleeve vertical

We had to stop at this point because my 20-year-old 1/2″ drill started smoking badly. I couldn’t really complain as I had gotten a LOT of use out of it, but it was apparently time for a new one. The next day I came back with a new 1/2″ hammer-drill and finished the foundation work. I poured concrete up to grade level so we could mow around it without having to come back and weed-whack the base.

The next step (literally) was to install the flag pole itself. My stepson was visiting from St. Louis, so helped me assemble the pole kit and set it into the sleeve.

And here is a short video of the completed flag pole with the U.S. flag proudly flying. Sure wish I had done this 20 years ago, but better late than never!

Proud to be an American!

Frank

Wall-E2’s many words for ‘obstacle’

Leave a reply

In a manner analogous to the many words for ‘ice’ and ‘snow’ in the Inuit language, Wall-E2 has many words for ‘obstacle’, as follows:

Wall-E2 gets too close to an obstacle in front
Wall-E2 gets too close to the currently-tracked wall during tracking operations
Wall-E2 senses an upcoming wall while tracking the current wall
Wall-E2 hits something that causes it to get stuck and stop moving forward but the obstacle doesn’t register on the forward sensor
Wall-E2 gets too close to an obstacle to the rear

Wall-E2 gets too close to an obstacle in front

If Wall-E2 isn’t currently tracking a wall and senses an upcoming obstacle, it stops, backs up, and turns away from the nearest wall. If there is no ‘nearest wall’ it simply turns one way or the other and hopes for the best ;-).

Wall-E2 gets too close to the currently-tracked wall during tracking operations

This item in the above list was an unintended consequence of Wall-E2’s new parallel offset tracking ability. After finding the parallel orientation at a distance greater than the desired offset, Wall-E2 makes a cut toward the wall to capture the offset. Unfortunately this also means the forward LIDAR measures the slant distance to the near wall, NOT the distance to any upcoming walls. In some cases this leads Wall-E2 to believe it is running into an obstacle, and instead of continuing the offset capture maneuver, it instead executes an obstacle avoidance maneuver. The fix for this problem is to recognize that when Wall-E2 is deliberately angling toward the wall, and reduce the ‘obstacle avoidance distance’ OAD accordingly. So, for a desired wall offset distance D_off and for a maximum approach angle corresponding to a steering value of -S_max (negative steering values are ‘toward the wall’), the ‘effective obstacle avoidance distance’ OAD_eff should be less by some factor, (arbitrarily selected here to be 2). This gives OAD_eff = D_off/2 for a steering value of -S_max, linearly increasing to D_off when S = 0. This is a straight line with slope 1/(2*S_max) and y-intercept of D_off for all S values <= 0. For S > 0, OAD_eff = D_off. Here’s a graph of the equation for S_tgt between 0 & -0.3

This image has an empty alt attribute; its file name is 201112-Calc-OAD-vs-SteerValTgt.jpg

Wall-E2 senses an upcoming wall while tracking the current wall

When tracking a wall, which by definition means in MODE_WALLTRACKING with a target steering value between -S and +S, Wall-E2’s desired response to an upcoming wall is to stop and make a 90º ‘spin turn’ away from the currently-being-tracked wall at the desired wall tracking offset value, on the theory that this will place Wall-E2 in the right place to start tracking the next wall.

Wall-E2 hits something that causes it to get stuck and stop moving forward but the obstacle doesn’t register on the forward sensor

This is the classic ‘I’m stuck!’ situation detected by monitoring the mathematical variance of front distance measurements over time, as described in this post. The recovery technique used up to now has been to back up for 1-2 seconds, and then turn 90º However, recovery from this condition can be problematic, as the robot will sometimes run backwards into another obstacle and execute the reverse ‘tractor scare maneuver’ of ‘Cars’ fame. With the addition of a 7th VL53L0X time-of-flight sensor, Wall-E2 can now detect a rear obstacle before it tries to climb it, leading to a better experience for the robot and it’s owner (that would be me). Now the ‘ExecuteStuckRecoveryManeuver()’ function first assesses whether or not there is enough room behind to successfully back up. If there is, it does the following:

Backs up a predetermined distance using the most suitable (forward or rear) distance sensor
Makes a 90º turn away from nearest wall
Moves forward a predetermined distance using either the forward or rear distance sensor
Makes another 90º turn to parallel the nearest wall again

The idea here is that if Wall-E2 is stuck on a shoe or a chair leg, it might be able to go around it and continue wall tracking. If not, it can repeat this procedure until it eventually works its way around the obstacle.

Wall-E2 gets too close to an obstacle to the rear

This condition rarely/never occurs in isolation; it happens during one of the other obstacle avoidance maneuvers, and only then when Wall-E2 fails to check it’s rear clearance before starting to back up.

A Status Byte/Word for Wall-E3?

Leave a reply

Posted 25 May 2024

Lately I have been working (or re-working) on Wall-E3’s ‘MoveToDesiredFrontDistCm()’ and ‘MoveToDesiredRearDistCm()’ capability. As I worked on these functions, I realized that the failure modes weren’t particularly straightforward. Both the ‘Front’ and ‘Rear’ functions can direct forward or rearward movement to achieve the desired distance, so in theory either function could experience a ‘stuck’ condition in either direction – ugh! In addition, I’m not sure what to do if either function experiences a ‘front obstacle’, ‘front offset distance’ or ‘rear obstacle’ condition.

Currently I have a set of enums that describe expected anomaly conditions, as shown below.

enum AnomalyCode
{
  ANOMALY_NONE = 0,
  ANOMALY_STUCK_AHEAD,
  ANOMALY_STUCK_BEHIND,
  ANOMALY_OBSTACLE_AHEAD,
  ANOMALY_WALL_OFFSET_DIST_AHEAD,
  ANOMALY_OBSTACLE_BEHIND,
  ANOMALY_DEAD_BATTERY,
  ANOMALY_CHARGER_CONNECTED, //added 05/22/22
  ANOMALY_EXCESS_STEER_VAL, //added 05/19/23
  ANOMALY_IR_BEAM_AVAILABLE, //added 08/20/23
  ANOMALY_MIRRORED_SFC //added 01/21/24
};

enum AnomalyCode

{

ANOMALY_NONE = 0,

ANOMALY_STUCK_AHEAD,

ANOMALY_STUCK_BEHIND,

ANOMALY_OBSTACLE_AHEAD,

ANOMALY_WALL_OFFSET_DIST_AHEAD,

ANOMALY_OBSTACLE_BEHIND,

ANOMALY_DEAD_BATTERY,

ANOMALY_CHARGER_CONNECTED, //added 05/22/22

ANOMALY_EXCESS_STEER_VAL, //added 05/19/23

ANOMALY_IR_BEAM_AVAILABLE, //added 08/20/23

ANOMALY_MIRRORED_SFC //added 01/21/24

};

And, in the current version of the ‘MoveTo..’ functions, the above are used as exit conditions from the movement loop, like the following snippet:

while (abs(gl_FrontCm - offsetCm) > 0.01 * MAX_MOVE_TO_DIST_ERROR_PCT * offsetCm 
&& gl_LastAnomalyCode != ANOMALY_STUCK_AHEAD
  && gl_LastAnomalyCode != ANOMALY_STUCK_BEHIND)

while (abs(gl_FrontCm - offsetCm) > 0.01 * MAX_MOVE_TO_DIST_ERROR_PCT * offsetCm

&& gl_LastAnomalyCode != ANOMALY_STUCK_AHEAD

&& gl_LastAnomalyCode != ANOMALY_STUCK_BEHIND)

gl_LastAnomalyCode is updated in the ‘UpdateAllEnvironmentParameters()’ function as shown below

void UpdateAllEnvironmentParameters(WallTrackingCases trkdir)
{
  //Purpose: Update all 'global' distance measurements, distance arrays, variances and anomaly variables
  //Provenance: Created 03/07/22
  //Inputs:
  // Left/Right/Rear/Front distance sensors
  //Outputs:
  // left rear/ctr/front, right rear/ctr/front, rear, front distance vars updated
  // Front and Rear variance vars updated
  // All boolean anomaly detection states updated
  //Plan:
  //  Step1: Update all left/right/rear/front distances
  //  Step2: Update all distance arrays
  //  Step3: Update both front and rear variance calcs
  //  Step4: Update all anomaly detection boolean vars
  //Notes:
  //  03/07/22 another attempt to corral the updates to 'global' environment vars
  //  08/20/23 repl all 'gl_bXXX = ' with 'if(???) gl_LastAnomalyCode = ANOMALY_XXX;'
  //  09/03/23 added UpdateIMUHdgValDeg() call 
  //  05/19/24 just discovered that ANOMALY_WALL_OFFSET_DIST_AHEAD anomaly isn't cleared when the robot moves from
  //    inside the offset distance to outside - ouch!  Added 'else {gl_LastAnomalyCode = ANOMALY_NONE}' to bottom of checks.
  //  05/19/24 bugfix: chg 'gl_FrontCm < REAR_OBSTACLE_DETECTION_DIST_CM' to 'gl_RearCm < REAR_OBSTACLE_DETECTION_DIST_CM'
// 
  //DEBUG!!
  //gl_pSerPort->printf("%lu: UpdateAllEnvironmentParameters(%s) with anomaly code = %s\n", millis(), WallTrackStrArray[trkdir], AnomalyStrArray[gl_LastAnomalyCode]);
  //DEBUG!!

  digitalWrite(DURATION_MEASUREMENT_PIN1, HIGH);//start measurement pulse

  //Step1: Update all left/right/rear/front distances
  UpdateAllDistances();

  UpdateIMUHdgValDeg(); //added 09/03/23

  gl_AvgHeadingDeg = UpdateAvgHdgArray(IMUHdgValDeg);

  //gl_pSerPort->printf("in UpdateAllEnv()\n");
  //gl_pSerPort->printf("Front\tRear\tLeftCtr\tRtCtr\n");
  //gl_pSerPort->printf("%d\t%2.1f\t%2.1f\t%2.1f\n", gl_FrontCm, gl_RearCm, gl_LeftCenterCm, gl_RightCenterCm);

  ////Step2: Update all distance arrays
  // 05/19/22 no longer using incremental calcs, so no need for oldestfront/reardistval
  UpdateFrontDistanceArray(gl_FrontCm);
  UpdateRearDistanceArray(gl_RearCm);

  //Step3: Update both front and rear variance calcs
  gl_Frontvar = CalcBruteFrontDistArrayVariance();//05/19/22 bugfix
  gl_Rearvar = CalcBruteRearDistArrayVariance();//05/19/22 bugfix

  if (gl_FrontCm <= WALL_OFFSET_TGTDIST_CM) gl_LastAnomalyCode = ANOMALY_WALL_OFFSET_DIST_AHEAD;
  else if (IsExcessiveSteerVal(trkdir)) gl_LastAnomalyCode = ANOMALY_EXCESS_STEER_VAL;//08/22/23 moved here from below

  else if (gl_FrontCm < FRONT_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_AHEAD;
  //else if (gl_FrontCm < REAR_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND;
  else if (gl_RearCm < REAR_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND;//05/19/24 bugfix

  else if (IsChargerConnected(gl_bChgConnect)) gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED;//10/30/23 moved above IsIRBeamAvail() 
  else if (IsIRBeamAvail()) gl_LastAnomalyCode = ANOMALY_IR_BEAM_AVAILABLE;
  //else if (IsChargerConnected(gl_bChgConnect)) gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED;
  else if (IsStuckBehind()) gl_LastAnomalyCode = ANOMALY_STUCK_BEHIND;
  else if (IsStuckAhead()) gl_LastAnomalyCode = ANOMALY_STUCK_AHEAD;
  else if (IsDeadBattery())
  {
    gl_LastAnomalyCode = ANOMALY_DEAD_BATTERY;
    gl_pSerPort->printf("Dead Battery detected with BattV = %2.2f\n", GetBattVoltage());
  }
  else { gl_LastAnomalyCode = ANOMALY_NONE; }//added 05/19/24

  digitalWrite(DURATION_MEASUREMENT_PIN1, LOW);//end measurement pulse

  //gl_pSerPort->printf("%lu: Bottom of UpdateAllEnvironmentParameters(%s) with anomaly code = %s\n", millis(), WallTrackStrArray[trkdir], AnomalyStrArray[gl_LastAnomalyCode]);
}

void UpdateAllEnvironmentParameters(WallTrackingCases trkdir)

{

//Purpose: Update all 'global' distance measurements, distance arrays, variances and anomaly variables

//Provenance: Created 03/07/22

//Inputs:

// Left/Right/Rear/Front distance sensors

//Outputs:

// left rear/ctr/front, right rear/ctr/front, rear, front distance vars updated

// Front and Rear variance vars updated

// All boolean anomaly detection states updated

//Plan:

// Step1: Update all left/right/rear/front distances

// Step2: Update all distance arrays

// Step3: Update both front and rear variance calcs

// Step4: Update all anomaly detection boolean vars

//Notes:

// 03/07/22 another attempt to corral the updates to 'global' environment vars

// 08/20/23 repl all 'gl_bXXX = ' with 'if(???) gl_LastAnomalyCode = ANOMALY_XXX;'

// 09/03/23 added UpdateIMUHdgValDeg() call

// 05/19/24 just discovered that ANOMALY_WALL_OFFSET_DIST_AHEAD anomaly isn't cleared when the robot moves from

// inside the offset distance to outside - ouch! Added 'else {gl_LastAnomalyCode = ANOMALY_NONE}' to bottom of checks.

// 05/19/24 bugfix: chg 'gl_FrontCm < REAR_OBSTACLE_DETECTION_DIST_CM' to 'gl_RearCm < REAR_OBSTACLE_DETECTION_DIST_CM'

//DEBUG!!

//gl_pSerPort->printf("%lu: UpdateAllEnvironmentParameters(%s) with anomaly code = %s\n", millis(), WallTrackStrArray[trkdir], AnomalyStrArray[gl_LastAnomalyCode]);

//DEBUG!!

digitalWrite(DURATION_MEASUREMENT_PIN1, HIGH);//start measurement pulse

//Step1: Update all left/right/rear/front distances

UpdateAllDistances();

UpdateIMUHdgValDeg(); //added 09/03/23

gl_AvgHeadingDeg = UpdateAvgHdgArray(IMUHdgValDeg);

//gl_pSerPort->printf("in UpdateAllEnv()\n");

//gl_pSerPort->printf("Front\tRear\tLeftCtr\tRtCtr\n");

//gl_pSerPort->printf("%d\t%2.1f\t%2.1f\t%2.1f\n", gl_FrontCm, gl_RearCm, gl_LeftCenterCm, gl_RightCenterCm);

////Step2: Update all distance arrays

// 05/19/22 no longer using incremental calcs, so no need for oldestfront/reardistval

UpdateFrontDistanceArray(gl_FrontCm);

UpdateRearDistanceArray(gl_RearCm);

//Step3: Update both front and rear variance calcs

gl_Frontvar = CalcBruteFrontDistArrayVariance();//05/19/22 bugfix

gl_Rearvar = CalcBruteRearDistArrayVariance();//05/19/22 bugfix

if (gl_FrontCm <= WALL_OFFSET_TGTDIST_CM) gl_LastAnomalyCode = ANOMALY_WALL_OFFSET_DIST_AHEAD;

else if (IsExcessiveSteerVal(trkdir)) gl_LastAnomalyCode = ANOMALY_EXCESS_STEER_VAL;//08/22/23 moved here from below

else if (gl_FrontCm < FRONT_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_AHEAD;

//else if (gl_FrontCm < REAR_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND;

else if (gl_RearCm < REAR_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND;//05/19/24 bugfix

else if (IsChargerConnected(gl_bChgConnect)) gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED;//10/30/23 moved above IsIRBeamAvail()

else if (IsIRBeamAvail()) gl_LastAnomalyCode = ANOMALY_IR_BEAM_AVAILABLE;

//else if (IsChargerConnected(gl_bChgConnect)) gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED;

else if (IsStuckBehind()) gl_LastAnomalyCode = ANOMALY_STUCK_BEHIND;

else if (IsStuckAhead()) gl_LastAnomalyCode = ANOMALY_STUCK_AHEAD;

else if (IsDeadBattery())

{

gl_LastAnomalyCode = ANOMALY_DEAD_BATTERY;

gl_pSerPort->printf("Dead Battery detected with BattV = %2.2f\n", GetBattVoltage());

}

else { gl_LastAnomalyCode = ANOMALY_NONE; }//added 05/19/24

digitalWrite(DURATION_MEASUREMENT_PIN1, LOW);//end measurement pulse

//gl_pSerPort->printf("%lu: Bottom of UpdateAllEnvironmentParameters(%s) with anomaly code = %s\n", millis(), WallTrackStrArray[trkdir], AnomalyStrArray[gl_LastAnomalyCode]);

}

This function retrieves all the current distance readings, the current heading, and the current reading from the charger IR beam detection sensor. Then it assigns one of the ANOMALY enum values to gl_LastAnomalyCode based on one or more direct or derived sensor values. For instance, the ‘STUCK_AHEAD’ or ‘STUCK_BEHIND’ codes are assigned based on the value computed for the ‘front’ or ‘rear’ variance calculations.

The problem (or at least what I *think* is a problem) is that gl_LastAnomalyCode can only contain one value, and in the case of more than one ‘anomaly’ existing at the same time, the value assigned to gl_LastAnomalyCode is the one encountered first, as a result of the ‘else’ – ‘else if’ structure.

So, I started thinking that maybe I should instead implement a status byte or status word with bit positions assigned to possible error/anomaly conditions. Instead of an ‘else’ – ‘else if’ structure, the ‘UpdateAllEnvironmentParameters()’ would simply update each bit in the status object independently of all the others. Then functions whose behavior gets modified by one or more anomalies can consult the status object to determine what to do.

I think this approach is much more robust and generalized than the current ‘only one anomaly type at a time’ approach, but it is also much more complicated, in at least three different ways:

Managing updates to the status object would require ‘bit diddling’ operations for clearing or setting individual bits.
Deciding what to do could become much more complicated. Currently the ‘gl_LastAnomalyCode’ can contain only one value, so any behavior modification decisions are basically CASE blocks. With a status object behavior could depend on more than one parameter.
sdaf;ljsadkf

I’m really intrigued by the power and generality of the status byte/word idea, but more than a little worried about ripping out the ANOMALY_CODE stuff and replacing it with status. Maybe I’ll just add the status byte/word stuff in parallel with the current setup and see how it goes.

Stay Tuned,

Frank

The Robot Rises Again – Adding Vision Processing

Leave a reply

Posted 04 April 2026

Well today I exceeded my boredom threshold, so something had to give. I worked out on my basketball court for an hour, flew a glider race in the Condor3 Soaring Flight Simulator, played an online bridge tournament on BBO, and I still hadn’t used up half the day. My workbench is empty, my two 3D printers are silent, and I can’t think of a thing to do. However, while I paced around with my hands in my pockets feeling sorry for myself, I caught sight of Wall-E3, my autonomous wall-following robot gathering dust in its charging cradle in the corner. That robot has gone through a number of iterations, starting with this post in early 2015 and ending with this post in late 2024. The goal of this almost decade-long project was to create a robot that could autonomously navigate around our house, scaring cats and humans alike and recharging itself whenever it got hungry. It did a really good job at the scaring part but unfortunately, I never got it to the point where it wouldn’t get stuck in one of our junk-filled guest bedrooms; close, but no cigar 😒.

Looking at my robot, I realized that in the intervening decade, a lot had changed in the indoor navigation realm, in particular the ability to exploit video camera images for navigation in reasonably constrained environments. So, I decided to revisit my robot project to see if I could utilize the new technology to get closer to my goal of true autonomous navigation.

Part of my interest in starting a new run at the household navigation project has been my positive experience over the last year with Grok, X’s AI. Grok, despite some real issues that need to be kept in mind, is a GREAT research assistant. In the past, I might spend most of a week working my way through internet links and going down various rabbit holes trying to solve a subtle software or hardware problem but Grok can do the same thing in a few seconds. At this point I knew next to nothing about vision-based navigation systems, but I knew for sure that Grok would be able to distill the netiverse down to reasonably digestible chunks pretty quickly.

I started with the following prompt for Grok:

 have a small 4-wheeled robot that uses front, side and rear distance sensors (LIDAR) along with angle information from a  6DOF solid-state gyro to autonomously navigate around my home.  It works OK but always seems to run into 'special cases' where it gets stuck.  I'm thinking of redoing the sensing package to use vision processing instead of just distances.  Can you help me get started?

have a small 4-wheeled robot that uses front, side and rear distance sensors (LIDAR) along with angle information from a 6DOF solid-state gyro to autonomously navigate around my home. It works OK but always seems to run into 'special cases' where it gets stuck. I'm thinking of redoing the sensing package to use vision processing instead of just distances. Can you help me get started?

In less than thirty seconds, Grok produced the following:

Absolutely—switching to vision processing is a smart move for handling those tricky “special cases” where pure distance sensors (like your LIDAR + gyro combo) fall short. Cameras give you way more information: color, texture, edges, and even semantic understanding (e.g., “that’s a chair leg” vs. just “something 30cm away”). This helps with low obstacles, reflective surfaces, dynamic items, or cluttered home environments that confuse LIDAR.

Since you already have a solid 6DOF gyro (IMU), you can fuse it with vision for excellent Visual-Inertial Odometry (VIO) or Visual SLAM (vSLAM). This is basically what modern robot vacuums do (vSLAM), but fully customizable for your wheeled robot.

Quick Reality Check Before Diving InVision isn’t perfect either:

Sensitive to lighting changes (solution: good indoor lights or IR-capable cameras).
Needs decent compute (Raspberry Pi 5 or equivalent works; older Pis may struggle with heavy SLAM).
Depth from vision is usually “good enough” indoors but not as precise as LIDAR in perfect conditions.

Hybrid tip: You can keep your existing LIDAR/gyro as a backup while testing vision.Recommended Hardware to Get Started (2026-Friendly Options)You’ll replace/add a camera that provides both color and depth (RGB-D) or stereo for 3D perception. Top practical picks for small wheeled robots:

Best overall: Luxonis OAK-D series (OAK-D Lite or Pro)
Budget depth option: Arducam T2 RGBD ToF Camera (~$100)
Classic: Intel RealSense D435/D455 (if you can find one) or similar RGB-D.

Mount the camera forward-facing (maybe add a second for 360° if budget allows). Keep your gyro—it will make odometry rock-solid when fused.What’s your current brain? (Raspberry Pi 4/5, Jetson, Arduino + companion computer, etc.?) Budget for the camera? That’ll let me refine this exactly.Software Stack – The Modern Way (ROS2 Recommended). The gold standard for this is ROS 2 (Robot Operating System) + Nav2 navigation stack. It handles mapping, localization, path planning, and obstacle avoidance out of the box.

Core vision libraries:

OpenCV → Basic start (edge detection, simple avoidance).
RTAB-Map or ORB-SLAM3 → Full visual SLAM (builds a 3D map while localizing).
VINS-Fusion or OpenVINS → Perfect VIO that fuses your existing 6DOF gyro with camera data (highly accurate odometry).

All run great on Pi 5 with Ubuntu 24.04 + ROS 2 Jazzy (or newer).

This was a lot like drinking from a firehose, but within an hour or so I had a Raspberry Pi 5 kit and two different cameras (a cheap one arriving tomorrow, and a not-so-cheap one arriving in another week or so) on order. In the meantime I asked Grok to hunt up some tutorials and examples so I could start getting up to speed on all things ROS 2 Jazzy Robot Operating System) and SLAM (Simultaneous Localization and Mapping).

Hopefully in the next week or so I will have some hardware & software to play with, and maybe make a step or two in the direction of upgrading WallE3 to WallE4.

08 April 2026 Update:

Worked with Grok to start adding vision capability to my autonomous 4-wheel wall-tracking robot. The plan is to use a Luxonis OAK-D-Lite Fixed-Focus camera and a Raspberry Pi5 running Ubuntu 24.04 and ROS (Robot Operating System) to add vision processing to the robot. Because there was a long lead time for the Luxonis camera, we started with a cheaper (and faster delivery) alternative, the Arducam ToF Camera. Grok insisted the Arducam would work as a near-term solution until the Luxonis camera arrived, but it didn’t work out quite that way. The Arducam worked OK with the default Pi5 OS without ROS, but when we tried to add ROS and the drivers for the Arducam, we ran into a dead end. Then Grok was *sure* that we could simply replace the Pi5 OS with Ubuntu 24.04 LTS and everything would work – – – NOT! So now I’m waiting for the Luxonis so we can move forward again.

When I braced Grok about the readily apparent difference between what Grok thought was going to work fine and the actual result, I asked Grok to search for ANY successful implementation using Pi5, ROS and Arducam, and Grok could not come up with a single confirmed successful implementation. This is a real problem with Grok, one that the devs should be concerned about, but nobody seems to care.

Grok now uses several ‘agents’ to independently research user questions, and the eventual answer is a consensus (I think) of their findings. I have suggested that at least one of the agents should be dedicated to attempting to falsify the results of the others, but I have seen nothing to indicate this has actually gotten through to the devs. In this case, all the ‘red team’ agent would have to do is perform the above search for successful implementations and come up with ZERO, thereby making it obvious that starting with the Arducam instead of waiting for the Luxonis was probably not going to end well. On a positive note, I asked Grok to do the same search with the Luxonis instead of the Arducam, and it came back with several confirmed successful implementations.

Until Grok implements the ‘red team’ facility, users need to be aware that Grok will quite happily drive a user off a cliff, insisting all the way down that “this next fix will surely solve the problem”. Has anyone else had his happen?

16 April 2026 Update:

A lot has happened since my last post on this subject. Working with Grok, we finally got the OAK-D camera working on the Pi5, with real-time imagery displayed on a monitor connected to the Pi5. Of course, this is a LOOOONG way from actually implementing visual navigation on a robot, but ya gotta start somewhere!

When considering how to move the Pi5 and Oak-D camera setup onto the robot, I decided to refresh my old 2-wheel (plus castering wheel) robot rather than trying to shoehorn the Pi5 and camera setup onto my 4-wheel robot. The 2-wheel robot already as a set of nice wheel motors, a pair of DRV8871 motor drivers and a battery/charger setup, and it isn’t being used for anything else at the moment. So, I stripped off the Uno Mega 2560 controller, installed the Pi5 and the camera at one end, and a Teensy 4.1 MCU on the other. I also installed an 8V-to-5V LDO voltage regulator that I had hanging around from a previous project. Here are some photos

Then I started working on the idea of installing magnets on the wheels and Hall-effect sensors on the chassis to produce wheel rotation rate information to integrate with vision processing. Based on a conversation with Grok, I ordered some A3144 Hall-effect sensors from Amazon. When they came in, I implemented a small plugboard circuit to test the sensor, as shown in the following short video:

After I got this working, I created a printed magnet carrier for the wheels, and a small perfboard circuit mount for the sensor. I tested the setup by running the small motor test program Grok wrote for the Teensy 4.1, and the results are shown in the following video:

LED illuminates when the sensor is triggered by a wheel magnet. Scope in the background shows the detection waveform

Here’s a scope trace grab of a typical half-speed wheel motor sequence:

From the above trace, the measured frequency is 4.823Hz. There are four magnets, which implies that the wheel is turning at about 1.2RPS, or about 72RPM. The wheel diameter is about 67mm, which gives a wheel circumference of about 21cm. At the 1.2 RPS rate, this works out to about 0.24m/s.

Of course I’m not anywhere near done yet, as I need two sensors per wheel to be able to determine rotation direction so three more sensor circuits, and a second wheel magnet carrier and magnets for the other wheel. Still though, it’s a good start!

22 April 2026 Update:

I finally got all four A3144 Hall-effect sensor modules and the second wheel magnet carrier mounted and tested. As shown in the following photos, The Hall-effect sensor modules are mounted approximately 90 degrees from each other to provide effective direction sensing.

23 April 2026 Update:

After finishing the Hall-effect sensor project, I worked on implementing a wireless serial connection to the robot for remote status monitoring and for remote ‘Over The Air’ (OTA) programming. I have had this capability for years on the 4-wheel robot and it really paid off, so I didn’t want to get too far down the road with the 2-wheel version without having it. As Grok pointed out, I could use the wireless ethernet capability of the Raspberry Pi5 for this; wire a serial port on the Pi to a hardware serial port on the Teensy 4.1 and send updated .HEX files to the Teensy that way. However, I decided not to do this for three reasons; first and foremost, I disliked the idea of tying Teensy 4.1 program updates to a working Pi5, as that added another huge set of variables; secondly, the Pi5 draws a LOT of power and so I don’t want to have it running until absolutely required, and thirdly I completely solved the Teensy 3x/4x OTA problem some five years back (see this post, this post and this post) and can readily port the solution to the current configuration.

I mounted one of my spare HC-05 Bluetooth Serial modules to the back of my new camera support structure with hot glue and connected it to Serial2 (pins 7 & 8) on the Teensy 4.1. After the usual cussing and screwups getting the Tx & Rx lines connected properly, I was able to demonstrate simple pass-through serial-to-serial capability, so then I moved on to getting OTA working. First I created an Arduino project called ‘2WheelRobot_HC05_OTA1’. Then I cloned Joe Pasquariello’s FlasherX repository to my PC and copied the latest versions of FlashTxx.h/.cpp, FxUtil.h/.cpp, and FlasherTxx.ino from my local repo to the local folder of that project. Next I ported the ‘CheckForUserInput()’ function from one of my 4Wheel robot projects into the OTA1 project, removing everything but the ‘U’ (Update Firmware) case. After the usual number of mistakes and cussing, I got this working to the point where I could manually send a new .HEX file to the Teensy 4.1 using the HC-05 Bluetooth serial link and TerraTerm, as shown in the following screengrab:

The next step is to automate this procedure as I did back in 2021. I should be able to use the same ‘board.txt’ and ‘TeensyOTA1.ttl’ files from back then. At this point I decided to try my ‘symlink’ trick instead of copying the files directly. Here is the relevant text from ‘Symbolic Link HOWTO.txt’:

//04/19/24 Arduino projects now use an extra folder layer, so instead of

mklink C:\Users\Frank\Documents\Arduino\230918_WallE3_MoveTo_Test_V1\TeensyOTA1.ttl “C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl”
mklink C:\Users\Frank\Documents\Arduino\230918_WallE3_MoveTo_Test_V1\board.txt “C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt”

The commands need to be:

mklink C:\Users\Frank\Documents\Arduino\230918_WallE3_MoveTo_Test_V1\230918_WallE3_MoveTo_Test_V1\TeensyOTA1.ttl “C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl”
mklink C:\Users\Frank\Documents\Arduino\230918_WallE3_MoveTo_Test_V1\230918_WallE3_MoveTo_Test_V1\board.txt “C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt”

I opened a command prompt and pasted the first mklink command in, and then edited it to point to my ‘2WheelRobot_HC05_OTA1’ folder. When I was done, I had this command line:

C:\Windows\System32>mklink C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\TeensyOTA1.ttl "C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl"
symbolic link created for C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\TeensyOTA1.ttl <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl

C:\Windows\System32>mklink C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\board.txt "C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt"
symbolic link created for C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\board.txt <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt

C:\Windows\System32>mklink C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\TeensyOTA1.ttl "C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl"

symbolic link created for C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\TeensyOTA1.ttl <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl

C:\Windows\System32>mklink C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\board.txt "C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt"

symbolic link created for C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1\board.txt <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt

After executing these commands, I confirmed that the ‘board.txt’ and ‘TeensyOTA1.ttl’ files now appear in the ‘C:\Users\Frank\Documents\Arduino\2WheelRobot_HC05_OTA1\2WheelRobot_HC05_OTA1’ folder – yay!

To confirm proper operation, I changed the ‘LED_ONOFF_MSEC’ parameter from 250 to 1000 in the .ino file, changed the COM port to COM12 and hit F5.

Rats! Close – but no Cigar! It started off OK and got to the point where TeraTerm should send the HEX file, and then I got this error message:

Hmm, turns out it actually worked – now the LED is blinking at 1000mSec ON, 1000mSec OFF, instead of 250 ON/OFF. So I tried again and this time I got “unable to connect to Teensy on COM12

25 April 2026 Update:

With a lot of help from Grok, I finally got to the point where I can now do ‘over-the-air’ (OTA) program updates to my 2-wheel robot via the onboard HC-05 BT serial port connected to the Teensy 4.1 Serial2 port (pins 7 & 8). For all the details, see the ’25 April 2026 Update’ section in this post.

At this point in the project, I have a 2 wheel robot with the following characteristics:

Teensy 4.1 processor and HC-05 BT Serial (Serial2) allowing OTA program updates
Dual DRV8871 motor controllers
7.4V (dual Lithium-Oxide) battery pack with 5V LDO voltage regulator
Raspberry Pi5 micro-computer with ROS and other relevant software installed
Luxonis OAK-D Lite camera connected to the Pi5 and functionally tested
4ea A3144 Hall-effect sensors mounted in physically quadrature pairs on each wheel, opposite a 4-magnet disc for speed & direction sensing.

At the moment the robot has no distance sensing capability, although the plan is to mount a pair of VL53L1X ToF sensors for side-distance monitoring.

the Pi5 and camera are not yet connected, as their current draw is significant and may require an upgraded battery and regulator for practical use. Here are some photos of the current robot configuration:

Here is the current system schematic. Note that theRaspberry Pi5, Luxonis OAK-D camera and MPU6050 6DoF gyro have not yet been incorporated into the system.

And here is the complete Teensy 4.1 sketch that was used to verify OTA programming capabilities.

/*
    Name:       2Wheel_HC05_OTA2.ino
    Created:	4/25/2026
    Author:     FRANK_XPS_9530\Frank
    Purpose:    OTA updates over HC05 Bluetooth on Serial2
    Notes:
      //04/25/26 this project uses the new (2022) FlashTxx/FXUtil file configuration
*/

//04/25/26 - can't do this here - must edit FlashTxx.h line 79-ish for this
//#define RAM_BUFFER_SIZE 0x40000   // 256 KB RAM buffer - fixes the "max address too large" error on Teensy 4.1


#include "FXUtil.h"          // hex parsing + update_firmware()
extern "C"
{
#include "FlashTxx.h"    // Teensy 4.1 flash primitives
}

uint32_t buffer_addr, buffer_size;

elapsedMillis MsecSinceLastLEDToggle;

void setup()
{
  //04/25/26 can't wait for Serial (hardwire USB) as it probably isn't connected.
  //Just use 2sec delay instead
  Serial.begin(115200);
  delay(2000);

  Serial2.begin(115200);          // HC05 on Serial2
  while (!Serial2) {}

  pinMode(LED_BUILTIN, OUTPUT);

  Serial2.println(F("\n=== HC05 OTA Ready on Serial2 - send 'U' to start update ==="));
  Serial2.flush();

  MsecSinceLastLEDToggle = 0;
}

void loop()
{
  if (Serial2.available())
  {
    CheckForUserInput();
  }

  if (MsecSinceLastLEDToggle >= 100)
  {
    MsecSinceLastLEDToggle -= 100;
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
}

//04/25/26 copied from 4-wheel robot project and removed all the other control cases.
//I will add the other cases for motor control back in later.
void CheckForUserInput()
{
  if (Serial2.available() > 0)
  {
    int incomingByte = Serial2.read();

    Serial2.print("I received: ");
    Serial2.println(incomingByte, HEX);

    switch (incomingByte)
    {
    case 0x55: // 'U'
    case 0x75: // 'u'
    {
      Serial2.println(F("Start Program Update - Send new HEX file!"));

      // exact string your TeensyOTA1.ttl macro expects
      Serial2.println(F("waiting for hex lines..."));

      if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
      {
        Serial2.printf("unable to create buffer\n");
        Serial2.flush();
        for (;;) {}
      }

      Serial2.printf("buffer = %1luK %s (%08lX - %08lX)\n",
        buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",
        buffer_addr, buffer_addr + buffer_size);

      while (Serial2.available())
      {
        Serial2.read();
      }

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

      Serial2.printf("erase FLASH buffer / free RAM buffer...\n");
      firmware_buffer_free(buffer_addr, buffer_size);
      Serial2.flush();
      REBOOT;
      break;
    }
    }
  }
}

100

101

102

103

Name: 2Wheel_HC05_OTA2.ino

Created: 4/25/2026

Author: FRANK_XPS_9530\Frank

Purpose: OTA updates over HC05 Bluetooth on Serial2

Notes:

//04/25/26 this project uses the new (2022) FlashTxx/FXUtil file configuration

//04/25/26 - can't do this here - must edit FlashTxx.h line 79-ish for this

//#define RAM_BUFFER_SIZE 0x40000 // 256 KB RAM buffer - fixes the "max address too large" error on Teensy 4.1

#include "FXUtil.h" // hex parsing + update_firmware()

extern "C"

{

#include "FlashTxx.h" // Teensy 4.1 flash primitives

}

uint32_t buffer_addr, buffer_size;

elapsedMillis MsecSinceLastLEDToggle;

void setup()

{

//04/25/26 can't wait for Serial (hardwire USB) as it probably isn't connected.

//Just use 2sec delay instead

Serial.begin(115200);

delay(2000);

Serial2.begin(115200); // HC05 on Serial2

while (!Serial2) {}

pinMode(LED_BUILTIN, OUTPUT);

Serial2.println(F("\n=== HC05 OTA Ready on Serial2 - send 'U' to start update ==="));

Serial2.flush();

MsecSinceLastLEDToggle = 0;

}

void loop()

{

if (Serial2.available())

{

CheckForUserInput();

}

if (MsecSinceLastLEDToggle >= 100)

{

MsecSinceLastLEDToggle -= 100;

digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

}

//04/25/26 copied from 4-wheel robot project and removed all the other control cases.

//I will add the other cases for motor control back in later.

void CheckForUserInput()

{

if (Serial2.available() > 0)

{

int incomingByte = Serial2.read();

Serial2.print("I received: ");

Serial2.println(incomingByte, HEX);

switch (incomingByte)

{

case 0x55: // 'U'

case 0x75: // 'u'

{

Serial2.println(F("Start Program Update - Send new HEX file!"));

// exact string your TeensyOTA1.ttl macro expects

Serial2.println(F("waiting for hex lines..."));

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

{

Serial2.printf("unable to create buffer\n");

Serial2.flush();

for (;;) {}

}

Serial2.printf("buffer = %1luK %s (%08lX - %08lX)\n",

buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",

buffer_addr, buffer_addr + buffer_size);

while (Serial2.available())

{

Serial2.read();

}

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

Serial2.printf("erase FLASH buffer / free RAM buffer...\n");

firmware_buffer_free(buffer_addr, buffer_size);

Serial2.flush();

REBOOT;

break;

}

27 April 2026 Update:

I ran into a problem with the way the ‘quadrature’ Hall-effect sensor module was mounted on the wheel motors. I had simply hot-glued them to the motor casing, but after a day or so the module fell off, so I needed something a little more permanent.

I designed a printed piece to snap-fit onto the motor body, with a slot in the top for the Hall-effect sensor perfboard, as shown below:

Here is a photo showing the Hall-effect sensor modules mounted on the underside of the robot.

02 May 2026 Update:

As it turns out, the above ‘physical quadrature’ arrangement was optimally bad, because it does not provide the 00 01 10 11 activation sequence required for accurate speed and direction sensing. After figuring this out, I wound up manually adjusting the position of one of the sensor modules to get the proper sequence, and then hot gluing the module down at that point. The following photo shows the somewhat messy current arrangement.

My plan is to design another version of the motor mount for a more permanent installation.

I also spent some time neatening up the topside of the robot and updating the schematic, as shown in the following photos.

04 May 2026 Update:

Grok and I worked together to remove a lot of unneeded PID code and add forward & reverse tick-count based motion control. Now the robot will move forward or backward a specified distance with a command such as “D, 0.3” or “D, -0.5” (meters). It will also stop in response to an ‘S’ command. Here’s the full code:

/*
    Name:       2WheelEncoderNoPID_OTA.ino
    Created:	5/2/2026 5:55:27 PM
    Author:     FRANK_XPS_9530\Frank
*/

// ================================================================
//  Fixed PWM + Tick-Based Odometry - Teensy 4.1
//  Minimal, reliable, no PID. Encoders renamed to leftTick/rightTick
//  Date: 28 April 2026
// ================================================================

//05/03/2026  Added OTA and positive/negative movement capability

#include <Arduino.h>
#include <Encoder.h>

#include "FXUtil.h"          // hex parsing + update_firmware()
extern "C"
{
#include "FlashTxx.h"    // Teensy 4.1 flash primitives
}

uint32_t buffer_addr, buffer_size;


// ------------------- PIN DEFINITIONS -------------------
#define LEFT_ENC_A   30
#define LEFT_ENC_B   29
#define RIGHT_ENC_A  32
#define RIGHT_ENC_B  31

#define LEFT_IN1     11
#define LEFT_IN2     12
#define RIGHT_IN1     9
#define RIGHT_IN2    10

#define LED_PIN      13

// ------------------- TUNABLES (your proven values) -------------------
const int   DRIVE_PWM = 175;      // base PWM
const int   PWM_OFFSET = 7;        // left +7, right -7 → nearly straight
const float WHEEL_RADIUS_MM = 33.0;
const float TICKS_PER_REV = 16.0;


// ------------------- ENCODERS (renamed as requested) -------------------
Encoder leftTick(LEFT_ENC_A, LEFT_ENC_B);
Encoder rightTick(RIGHT_ENC_A, RIGHT_ENC_B);

  //  11/21/21 rev to accomodate cmd input from either serial, and output to both
  //  12/25/21 another try at using pointer for Serial object
  Stream* pStream = 0;

#define TELEMETRY_PERIOD_MSEC 100

  long startLeftTicks = 0;
  long startRightTicks = 0;

  // ------------------- GLOBALS -------------------
  bool distanceMode = false;
  long targetTicks = 0;
  long travelTicks = 0;

  char cmdBuffer[64];
  uint8_t cmdIdx = 0;

  bool bStopped = true;
  bool bForward = true;

void setup()
{
  //04/25/26 can't wait for Serial (hardwire USB) as it probably isn't connected.
  //Just use 2sec delay instead
  Serial.begin(115200);
  delay(2000);

  //Serial2 via HC-05 BT link
  Serial2.begin(115200);          // HC05 on Serial2
  while (!Serial2) {}

  //05/02/26 set pointer to Serial or Serial2 and then use the pointer for output
  if (Serial)
  {
    pStream = (Stream*)&Serial;
    pStream->printf("pStream set to Serial\n");

  }
  else if (Serial2)
  {
    pStream = (Stream*)&Serial2;
    pStream->printf("pStream set to Serial2\n");
  }
  else
  {
    pStream = 0;
  }


  if (pStream)
  {
    pStream->println("Fixed PWM + Tick-Based Odometry with HC-05 OTA");
    pStream->println("Commands: D,distance_m   or   S");

    pStream->printf("Msec\tL_Tick\tR_Tick\n");
    pStream->println(F("\n=== HC05 OTA Ready on Serial2 - send 'U' to start update ==="));
    pStream->flush();
  }

  pinMode(LED_PIN, OUTPUT);

  pinMode(LEFT_IN1, OUTPUT);  pinMode(LEFT_IN2, OUTPUT);
  pinMode(RIGHT_IN1, OUTPUT);  pinMode(RIGHT_IN2, OUTPUT);
}

void loop()
{
  unsigned long now = millis();

  long avgTicks = getAvgTicks();

  // Telemetry every Telemetry_Period_ms
  if (now % TELEMETRY_PERIOD_MSEC == 0 && !bStopped)
  {
    pStream->printf("%lu\t%ld\t%ld\n",
      now, leftTick.read(), rightTick.read());
  }
  // Distance reached?
  if (bForward && avgTicks >= targetTicks)
  {
    pStream->println("Target ticks reached - stopping!");
    StopBothMotors();
    bStopped = true;
  }
  else if (!bForward && avgTicks <= targetTicks)
  {
    pStream->println("Target ticks (%d) reached - stopping!");
    StopBothMotors();
    bStopped = true;
  }

  digitalWrite(LED_PIN, !digitalRead(LED_PIN));

  CheckForUserInput();
}

//04/25/26 copied from 4-wheel robot project and removed all the other control cases.
void CheckForUserInput()
{
  if (pStream->available() > 0)
  {
    int incomingByte = pStream->read();

    pStream->print("I received: ");
    pStream->println(incomingByte, HEX);
  //}

    switch (incomingByte)
    {
    case 0x44: // 'D' Move a specified distance
    case 0x64: // 'd'
    {
      String inputString = pStream->readString(); // Reads until timeout or buffer empty
      pStream->printf("You entered: %s\n", inputString.c_str());

      inputString.trim();                // Remove leading/trailing whitespace

      if (inputString.length() > 0)
      {
        // === ADAPTED PARSING STARTS HERE ===
        if (inputString.startsWith(","))
        {
          String numStr = inputString.substring(1);   // remove the leading comma
          numStr.trim();                              // remove any space after the comma

          float distance = numStr.toFloat();

          if (distance > 0.0f)
          {
            float circumference = 2.0f * 3.14159265f * (WHEEL_RADIUS_MM / 1000.0f);
            float targetRevs = distance / circumference;
            //targetTicks = (long)(targetRevs * TICKS_PER_REV + 0.5f);
            travelTicks = (long)(targetRevs * TICKS_PER_REV + 0.5f);

            startLeftTicks = leftTick.read();
            startRightTicks = rightTick.read();
            targetTicks = getAvgTicks() + travelTicks;
            distanceMode = true;

            //pStream->printf("Driving %.3f m (%ld ticks)\n", distance, targetTicks);
            pStream->printf("Driving %.3f m (%ld ticks)\n", distance, targetTicks);

            //05/03/26 now using MoveForward/Reverse functions
            MoveForward(DRIVE_PWM + PWM_OFFSET, DRIVE_PWM - PWM_OFFSET);

            bStopped = false;
            bForward = true;

          }
          else if (distance < 0.0f)
          {
            float circumference = 2.0f * 3.14159265f * (WHEEL_RADIUS_MM / 1000.0f);
            float targetRevs = distance / circumference;
            //targetTicks = (long)(targetRevs * TICKS_PER_REV + 0.5f);
            targetTicks = (long)(targetRevs * TICKS_PER_REV - 0.5f);

            startLeftTicks = leftTick.read();
            startRightTicks = rightTick.read();
            distanceMode = true;

            pStream->printf("Driving %.3f m (%ld ticks)\n", distance, targetTicks);

            //05/03/26 now using MoveForward/Reverse functions
            MoveReverse(DRIVE_PWM - PWM_OFFSET, DRIVE_PWM + PWM_OFFSET);

            bStopped = false;
            bForward = false;

          }
          else
          {
            pStream->println("Error: Distance must be greater than or less than 0.");
          }
        }
        else
        {
          pStream->println("Invalid format. Expected ,<distance> after D/d");
          pStream->printf("Received after D/d: '%s'\n", inputString.c_str());
        }
        // === ADAPTED PARSING ENDS HERE ===
      }
      else
      {
        pStream->println("No valid input received.");
      }
      break;   // always good practice
    }    
    case 0x53: // 'S' Stop both motors
    case 0x73: // 's'
    {
      pStream->println("Received:S -- Stopping!");
      StopBothMotors();
      bStopped = true;
      while (1) { digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(500); }
    }
    case 0x55: // 'U'
    case 0x75: // 'u'
    {
      pStream->println(F("Start Program Update - Send new HEX file!"));

      // exact string your TeensyOTA1.ttl macro expects
      pStream->println(F("waiting for hex lines..."));

      if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
      {
        pStream->printf("unable to create buffer\n");
        pStream->flush();
        for (;;) {}
      }

      pStream->printf("buffer = %1luK %s (%08lX - %08lX)\n",
        buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",
        buffer_addr, buffer_addr + buffer_size);

      while (pStream->available())
      {
        pStream->read();
      }

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

      pStream->printf("erase FLASH buffer / free RAM buffer...\n");
      firmware_buffer_free(buffer_addr, buffer_size);
      pStream->flush();
      REBOOT;
      break;
    }
    }
  }
}

void StopBothMotors()
{
  analogWrite(LEFT_IN1, 0);
  analogWrite(LEFT_IN2, 0);
  analogWrite(RIGHT_IN1, 0);
  analogWrite(RIGHT_IN2, 0);
}

//05/03/26 copied from T35_WallE3_V4.ino & adapted for DRV8871 drivers
void MoveForward(int leftspeednum, int rightspeednum)
{
  //Purpose:  Move ahead continuously
  //Provenence: G. Frank Paynter 05/03/2026
  //Inputs:  
  //    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
  //    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
  //Outputs: both drive Motors energized with the specified speed
  //Notes:
    //05/03/26 copied from T35_WallE3_V4.ino & adapted for DRV8871 drivers

  analogWrite(LEFT_IN1, leftspeednum);
  digitalWrite(LEFT_IN2, LOW);

  analogWrite(RIGHT_IN1, leftspeednum);
  digitalWrite(RIGHT_IN2, LOW);
}


void MoveReverse(int leftspeednum, int rightspeednum)
{
  //Purpose:  Move ahead continuously
  //Provenence: G. Frank Paynter 05/03/2026
  //Inputs:  
  //    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
  //    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
  //Outputs: both drive Motors energized with the specified speed
  //Notes:
    //05/03/26 copied from T35_WallE3_V4.ino & adapted for DRV8871 drivers

  analogWrite(LEFT_IN2, leftspeednum);
  digitalWrite(LEFT_IN1, LOW);

  analogWrite(RIGHT_IN2, rightspeednum);
  digitalWrite(RIGHT_IN1, LOW);
}

long getAvgTicks()
{
  long curLeft = leftTick.read();
  long curRight = rightTick.read();
  long avg = (curLeft - startLeftTicks + curRight - startRightTicks) / 2;
  return avg;
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

Name: 2WheelEncoderNoPID_OTA.ino

Created: 5/2/2026 5:55:27 PM

Author: FRANK_XPS_9530\Frank

// ================================================================

// Fixed PWM + Tick-Based Odometry - Teensy 4.1

// Minimal, reliable, no PID. Encoders renamed to leftTick/rightTick

// Date: 28 April 2026

// ================================================================

//05/03/2026 Added OTA and positive/negative movement capability

#include <Arduino.h>

#include <Encoder.h>

#include "FXUtil.h" // hex parsing + update_firmware()

extern "C"

{

#include "FlashTxx.h" // Teensy 4.1 flash primitives

}

uint32_t buffer_addr, buffer_size;

// ------------------- PIN DEFINITIONS -------------------

#define LEFT_ENC_A 30

#define LEFT_ENC_B 29

#define RIGHT_ENC_A 32

#define RIGHT_ENC_B 31

#define LEFT_IN1 11

#define LEFT_IN2 12

#define RIGHT_IN1 9

#define RIGHT_IN2 10

#define LED_PIN 13

// ------------------- TUNABLES (your proven values) -------------------

const int DRIVE_PWM = 175; // base PWM

const int PWM_OFFSET = 7; // left +7, right -7 → nearly straight

const float WHEEL_RADIUS_MM = 33.0;

const float TICKS_PER_REV = 16.0;

// ------------------- ENCODERS (renamed as requested) -------------------

Encoder leftTick(LEFT_ENC_A, LEFT_ENC_B);

Encoder rightTick(RIGHT_ENC_A, RIGHT_ENC_B);

// 11/21/21 rev to accomodate cmd input from either serial, and output to both

// 12/25/21 another try at using pointer for Serial object

Stream* pStream = 0;

#define TELEMETRY_PERIOD_MSEC 100

long startLeftTicks = 0;

long startRightTicks = 0;

// ------------------- GLOBALS -------------------

bool distanceMode = false;

long targetTicks = 0;

long travelTicks = 0;

char cmdBuffer[64];

uint8_t cmdIdx = 0;

bool bStopped = true;

bool bForward = true;

void setup()

{

//04/25/26 can't wait for Serial (hardwire USB) as it probably isn't connected.

//Just use 2sec delay instead

Serial.begin(115200);

delay(2000);

//Serial2 via HC-05 BT link

Serial2.begin(115200); // HC05 on Serial2

while (!Serial2) {}

//05/02/26 set pointer to Serial or Serial2 and then use the pointer for output

if (Serial)

{

pStream = (Stream*)&Serial;

pStream->printf("pStream set to Serial\n");

}

else if (Serial2)

{

pStream = (Stream*)&Serial2;

pStream->printf("pStream set to Serial2\n");

}

else

{

pStream = 0;

}

if (pStream)

{

pStream->println("Fixed PWM + Tick-Based Odometry with HC-05 OTA");

pStream->println("Commands: D,distance_m or S");

pStream->printf("Msec\tL_Tick\tR_Tick\n");

pStream->println(F("\n=== HC05 OTA Ready on Serial2 - send 'U' to start update ==="));

pStream->flush();

}

pinMode(LED_PIN, OUTPUT);

pinMode(LEFT_IN1, OUTPUT); pinMode(LEFT_IN2, OUTPUT);

pinMode(RIGHT_IN1, OUTPUT); pinMode(RIGHT_IN2, OUTPUT);

}

void loop()

{

unsigned long now = millis();

long avgTicks = getAvgTicks();

// Telemetry every Telemetry_Period_ms

if (now % TELEMETRY_PERIOD_MSEC == 0 && !bStopped)

{

pStream->printf("%lu\t%ld\t%ld\n",

now, leftTick.read(), rightTick.read());

}

// Distance reached?

if (bForward && avgTicks >= targetTicks)

{

pStream->println("Target ticks reached - stopping!");

StopBothMotors();

bStopped = true;

}

else if (!bForward && avgTicks <= targetTicks)

{

pStream->println("Target ticks (%d) reached - stopping!");

StopBothMotors();

bStopped = true;

}

digitalWrite(LED_PIN, !digitalRead(LED_PIN));

CheckForUserInput();

}

//04/25/26 copied from 4-wheel robot project and removed all the other control cases.

void CheckForUserInput()

{

if (pStream->available() > 0)

{

int incomingByte = pStream->read();

pStream->print("I received: ");

pStream->println(incomingByte, HEX);

//}

switch (incomingByte)

{

case 0x44: // 'D' Move a specified distance

case 0x64: // 'd'

{

String inputString = pStream->readString(); // Reads until timeout or buffer empty

pStream->printf("You entered: %s\n", inputString.c_str());

inputString.trim(); // Remove leading/trailing whitespace

if (inputString.length() > 0)

{

// === ADAPTED PARSING STARTS HERE ===

if (inputString.startsWith(","))

{

String numStr = inputString.substring(1); // remove the leading comma

numStr.trim(); // remove any space after the comma

float distance = numStr.toFloat();

if (distance > 0.0f)

{

float circumference = 2.0f * 3.14159265f * (WHEEL_RADIUS_MM / 1000.0f);

float targetRevs = distance / circumference;

//targetTicks = (long)(targetRevs * TICKS_PER_REV + 0.5f);

travelTicks = (long)(targetRevs * TICKS_PER_REV + 0.5f);

startLeftTicks = leftTick.read();

startRightTicks = rightTick.read();

targetTicks = getAvgTicks() + travelTicks;

distanceMode = true;

//pStream->printf("Driving %.3f m (%ld ticks)\n", distance, targetTicks);

pStream->printf("Driving %.3f m (%ld ticks)\n", distance, targetTicks);

//05/03/26 now using MoveForward/Reverse functions

MoveForward(DRIVE_PWM + PWM_OFFSET, DRIVE_PWM - PWM_OFFSET);

bStopped = false;

bForward = true;

}

else if (distance < 0.0f)

{

float circumference = 2.0f * 3.14159265f * (WHEEL_RADIUS_MM / 1000.0f);

float targetRevs = distance / circumference;

//targetTicks = (long)(targetRevs * TICKS_PER_REV + 0.5f);

targetTicks = (long)(targetRevs * TICKS_PER_REV - 0.5f);

startLeftTicks = leftTick.read();

startRightTicks = rightTick.read();

distanceMode = true;

pStream->printf("Driving %.3f m (%ld ticks)\n", distance, targetTicks);

//05/03/26 now using MoveForward/Reverse functions

MoveReverse(DRIVE_PWM - PWM_OFFSET, DRIVE_PWM + PWM_OFFSET);

bStopped = false;

bForward = false;

}

else

{

pStream->println("Error: Distance must be greater than or less than 0.");

}

else

{

pStream->println("Invalid format. Expected ,<distance> after D/d");

pStream->printf("Received after D/d: '%s'\n", inputString.c_str());

}

// === ADAPTED PARSING ENDS HERE ===

}

else

{

pStream->println("No valid input received.");

}

break; // always good practice

}

case 0x53: // 'S' Stop both motors

case 0x73: // 's'

{

pStream->println("Received:S -- Stopping!");

StopBothMotors();

bStopped = true;

while (1) { digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(500); }

}

case 0x55: // 'U'

case 0x75: // 'u'

{

pStream->println(F("Start Program Update - Send new HEX file!"));

// exact string your TeensyOTA1.ttl macro expects

pStream->println(F("waiting for hex lines..."));

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

{

pStream->printf("unable to create buffer\n");

pStream->flush();

for (;;) {}

}

pStream->printf("buffer = %1luK %s (%08lX - %08lX)\n",

buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",

buffer_addr, buffer_addr + buffer_size);

while (pStream->available())

{

pStream->read();

}

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

pStream->printf("erase FLASH buffer / free RAM buffer...\n");

firmware_buffer_free(buffer_addr, buffer_size);

pStream->flush();

REBOOT;

break;

}

void StopBothMotors()

{

analogWrite(LEFT_IN1, 0);

analogWrite(LEFT_IN2, 0);

analogWrite(RIGHT_IN1, 0);

analogWrite(RIGHT_IN2, 0);

}

//05/03/26 copied from T35_WallE3_V4.ino & adapted for DRV8871 drivers

void MoveForward(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//Provenence: G. Frank Paynter 05/03/2026

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Notes:

//05/03/26 copied from T35_WallE3_V4.ino & adapted for DRV8871 drivers

analogWrite(LEFT_IN1, leftspeednum);

digitalWrite(LEFT_IN2, LOW);

analogWrite(RIGHT_IN1, leftspeednum);

digitalWrite(RIGHT_IN2, LOW);

}

void MoveReverse(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//Provenence: G. Frank Paynter 05/03/2026

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Notes:

//05/03/26 copied from T35_WallE3_V4.ino & adapted for DRV8871 drivers

analogWrite(LEFT_IN2, leftspeednum);

digitalWrite(LEFT_IN1, LOW);

analogWrite(RIGHT_IN2, rightspeednum);

digitalWrite(RIGHT_IN1, LOW);

}

long getAvgTicks()

{

long curLeft = leftTick.read();

long curRight = rightTick.read();

long avg = (curLeft - startLeftTicks + curRight - startRightTicks) / 2;

return avg;

}

I also had Grok generate a Python script to automate the task of creating symlinks to my custom ‘board.txt’ and ‘TeensyOTA1.ttl’ files necessary for OTA updates. Now I can double-click a desktop shortcut, point to a folder, and have the above two files appear magically as symlinks to the files in my new “C:\Users\Frank\Documents\Arduino\Robot Common Files 2026” folder. This is wonderful time-saver and I only wish I had done this earlier. Here’s the code:

import sys
import os
import ctypes
from pathlib import Path
import subprocess
import tkinter as tk
from tkinter import filedialog, messagebox

def is_admin():
    try:
        return ctypes.windll.shell32.IsUserAnAdmin()
    except:
        return False

def run_as_admin():
    """Force UAC elevation if not already running as admin"""
    if is_admin():
        return
    
    script = sys.argv[0]
    try:
        ctypes.windll.shell32.ShellExecuteW(None, "runas", sys.executable, f'"{script}"', None, 1)
        sys.exit(0)          # exit the non-elevated instance
    except:
        messagebox.showerror("Elevation Failed", 
                           "Could not get Administrator rights.\n\n"
                           "Right-click the .py or .exe → Run as administrator.")
        sys.exit(1)

def select_target_folder():
    root = tk.Tk()
    root.withdraw()
    root.attributes('-topmost', True)
    
    folder = filedialog.askdirectory(
        title="Select your Arduino project folder",
        initialdir=r"C:\Users\Frank\Documents\Arduino"
    )
    root.destroy()
    return Path(folder) if folder else None

def create_symlinks(target_dir: Path):
    common_dir = Path(r"C:\Users\Frank\Documents\Arduino\Robot Common Files 2026")
    
    if not common_dir.exists():
        messagebox.showerror("Error", f"Common folder not found:\n{common_dir}")
        return False

    files_to_link = ["board.txt", "TeensyOTA1.ttl"]
    success_count = 0

    for fname in files_to_link:
        source_file = common_dir / fname
        link_file   = target_dir / fname

        # Remove old link if it exists
        if link_file.exists() or link_file.is_symlink():
            try:
                os.unlink(link_file)
                print(f"Removed existing link: {fname}")
            except Exception as e:
                print(f"Warning: Could not remove {fname}: {e}")

        if not source_file.exists():
            print(f"Warning: Source file missing: {source_file}")
            continue

        cmd = f'mklink "{link_file}" "{source_file}"'
        print(f"Running: {cmd}")
        
        try:
            result = subprocess.run(cmd, shell=True, capture_output=True, text=True)
            if result.returncode == 0:
                print(f"✓ Created: {fname}")
                success_count += 1
            else:
                print(f"✗ Failed: {fname}")
                print(result.stderr.strip())
        except Exception as e:
            print(f"✗ Exception: {e}")

    if success_count == len(files_to_link):
        messagebox.showinfo("Success!", 
                          f"All {success_count} symlinks created successfully!\n\n"
                          f"Target folder:\n{target_dir}")
        return True
    else:
        messagebox.showwarning("Partial Success", 
                             f"Only {success_count} of {len(files_to_link)} links created.")
        return False


if __name__ == "__main__":
    run_as_admin()          # ← This forces the UAC prompt every time
    
    print("=== Arduino Common Files Link Creator ===\n")
    
    target_dir = select_target_folder()
    if not target_dir:
        sys.exit(0)
    
    if not target_dir.exists():
        messagebox.showerror("Error", f"Folder does not exist:\n{target_dir}")
        sys.exit(1)
    
    create_symlinks(target_dir)
    
    input("\nPress Enter to close this window...")

100

101

102

103

104

105

106

107

108

import sys

import os

import ctypes

from pathlib import Path

import subprocess

import tkinter as tk

from tkinter import filedialog, messagebox

def is_admin():

try:

return ctypes.windll.shell32.IsUserAnAdmin()

except:

return False

def run_as_admin():

"""Force UAC elevation if not already running as admin"""

if is_admin():

return

script = sys.argv[0]

try:

ctypes.windll.shell32.ShellExecuteW(None, "runas", sys.executable, f'"{script}"', None, 1)

sys.exit(0) # exit the non-elevated instance

except:

messagebox.showerror("Elevation Failed",

"Could not get Administrator rights.\n\n"

"Right-click the .py or .exe → Run as administrator.")

sys.exit(1)

def select_target_folder():

root = tk.Tk()

root.withdraw()

root.attributes('-topmost', True)

folder = filedialog.askdirectory(

title="Select your Arduino project folder",

initialdir=r"C:\Users\Frank\Documents\Arduino"

)

root.destroy()

return Path(folder) if folder else None

def create_symlinks(target_dir: Path):

common_dir = Path(r"C:\Users\Frank\Documents\Arduino\Robot Common Files 2026")

if not common_dir.exists():

messagebox.showerror("Error", f"Common folder not found:\n{common_dir}")

return False

files_to_link = ["board.txt", "TeensyOTA1.ttl"]

success_count = 0

for fname in files_to_link:

source_file = common_dir / fname

link_file = target_dir / fname

# Remove old link if it exists

if link_file.exists() or link_file.is_symlink():

try:

os.unlink(link_file)

print(f"Removed existing link: {fname}")

except Exception as e:

print(f"Warning: Could not remove {fname}: {e}")

if not source_file.exists():

print(f"Warning: Source file missing: {source_file}")

continue

cmd = f'mklink "{link_file}" "{source_file}"'

print(f"Running: {cmd}")

try:

result = subprocess.run(cmd, shell=True, capture_output=True, text=True)

if result.returncode == 0:

print(f"✓ Created: {fname}")

success_count += 1

else:

print(f"✗ Failed: {fname}")

print(result.stderr.strip())

except Exception as e:

print(f"✗ Exception: {e}")

if success_count == len(files_to_link):

messagebox.showinfo("Success!",

f"All {success_count} symlinks created successfully!\n\n"

f"Target folder:\n{target_dir}")

return True

else:

messagebox.showwarning("Partial Success",

f"Only {success_count} of {len(files_to_link)} links created.")

return False

if __name__ == "__main__":

run_as_admin() # ← This forces the UAC prompt every time

print("=== Arduino Common Files Link Creator ===\n")

target_dir = select_target_folder()

if not target_dir:

sys.exit(0)

if not target_dir.exists():

messagebox.showerror("Error", f"Folder does not exist:\n{target_dir}")

sys.exit(1)

create_symlinks(target_dir)

input("\nPress Enter to close this window...")

05 May 2026 Update:

I added a tick-count reset ‘R/r’ operation to the available commands in the ‘CheckForUserInput() function. At this point the robot will go forwards and backwards by a set distance (tick count), will stop on command, and the tick count can be reset to zero at any time. Here is the ‘final’ code:

/*
    Name:       2WheelEncoderNoPID_OTA_Refactored.ino
    Author:     Frank Paynter (with refinements)
    Date:       2026-05-03
    Teensy 4.1 + DRV8871 + Encoders + HC-05 OTA
    Commands:   D,<meters>  (positive forward, negative reverse)
                S           (stop)
                U           (OTA update)

    Notes:
      05/04/26 added //#define NO_MOTORS so I can debug without motors running
*/

//#define NO_MOTORS //added 05/05/26


// ================================================================
//  PIN DEFINITIONS
// ================================================================
const uint8_t LEFT_ENC_A = 30;
const uint8_t LEFT_ENC_B = 29;
const uint8_t RIGHT_ENC_A = 32;
const uint8_t RIGHT_ENC_B = 31;

const uint8_t LEFT_IN1 = 11;
const uint8_t LEFT_IN2 = 12;
const uint8_t RIGHT_IN1 = 9;
const uint8_t RIGHT_IN2 = 10;

const uint8_t LED_PIN = 13;

// ================================================================
//  TUNABLES
// ================================================================
const int   BASE_PWM = 175;
const int   PWM_OFFSET = 7;
const float WHEEL_RADIUS_M = 0.033f;
const float TICKS_PER_REV = 16.0f;
const unsigned long TELEMETRY_MS = 100;

// ================================================================
//  INCLUDES AND GLOBALS
// ================================================================
#include <Arduino.h>
#include <Encoder.h>
#include "FXUtil.h"
extern "C"
{
#include "FlashTxx.h"
}

Encoder leftEnc(LEFT_ENC_A, LEFT_ENC_B);
Encoder rightEnc(RIGHT_ENC_A, RIGHT_ENC_B);

Stream* pStream = nullptr;

struct Odometry
{
  long startAvgTicks = 0;
  long targetDeltaTicks = 0;
  bool active = false;
  bool forward = true;
} odom;

unsigned long lastTelemetry = 0;

// ================================================================
//  MOTOR CONTROL
// ================================================================
void drive(int leftPWM, int rightPWM)
{
#ifndef NO_MOTORS
  if (leftPWM >= 0)
  {
    analogWrite(LEFT_IN1, leftPWM);
    digitalWrite(LEFT_IN2, LOW);
  }
  else
  {
    analogWrite(LEFT_IN2, -leftPWM);
    digitalWrite(LEFT_IN1, LOW);
  }

  if (rightPWM >= 0)
  {
    analogWrite(RIGHT_IN1, rightPWM);
    digitalWrite(RIGHT_IN2, LOW);
  }
  else
  {
    analogWrite(RIGHT_IN2, -rightPWM);
    digitalWrite(RIGHT_IN1, LOW);
  }
#endif // !NO_MOTORS
}

void stopMotors()
{
  //Serial2.println("In StopMotors()");   // keep your debug print

#ifndef NO_MOTORS
  // === ROBUST STOP FOR DRV8871 + Teensy 4.1 ===
  // 1. Explicitly kill any active PWM signals on ALL four pins
  analogWrite(LEFT_IN1, 0);
  analogWrite(LEFT_IN2, 0);
  analogWrite(RIGHT_IN1, 0);
  analogWrite(RIGHT_IN2, 0);

  // 2. Force both inputs LOW on each motor → clean coast stop
  digitalWrite(LEFT_IN1, LOW);
  digitalWrite(LEFT_IN2, LOW);
  digitalWrite(RIGHT_IN1, LOW);
  digitalWrite(RIGHT_IN2, LOW);
#endif // !NO_MOTORS
}
// ================================================================
//  ODOMETRY
// ================================================================
long getAvgTicks()
{
  return (leftEnc.read() + rightEnc.read()) / 2;
}

float metersToTicks(float meters)
{
  const float circ = 2.0f * 3.14159265f * WHEEL_RADIUS_M;
  return meters * (TICKS_PER_REV / circ);
}

// ================================================================
//  SETUP
// ================================================================
void setup()
{
  Serial.begin(115200);
  delay(2000);

  Serial2.begin(115200);
  while (!Serial2) {}

  pStream = Serial ? (Stream*)&Serial : (Stream*)&Serial2;

  pStream->println("\n=== 2-Wheel Encoder + OTA Ready ===");
  pStream->println("Commands: D,<meters>   S   U");
  pStream->println("Msec\tL_Tick\tR_Tick");

  pinMode(LED_PIN, OUTPUT);
  pinMode(LEFT_IN1, OUTPUT);
  pinMode(LEFT_IN2, OUTPUT);
  pinMode(RIGHT_IN1, OUTPUT);
  pinMode(RIGHT_IN2, OUTPUT);

  stopMotors();
}

// ================================================================
//  LOOP
// ================================================================
void loop()
{
  unsigned long now = millis();

  if (now - lastTelemetry >= TELEMETRY_MS)
  {
    if (odom.active)
    {
      pStream->printf("%lu\t%ld\t%ld\n", now, leftEnc.read(), rightEnc.read());
    }
    lastTelemetry = now;
  }

  if (odom.active)
  {
    long progress = getAvgTicks() - odom.startAvgTicks;
    bool reached = odom.forward ? (progress >= odom.targetDeltaTicks)
      : (progress <= odom.targetDeltaTicks);

    if (reached)
    {
      pStream->println("Target reached - stopping!");
      stopMotors();
      odom.active = false;
    }
  }

  digitalWrite(LED_PIN, !digitalRead(LED_PIN));

  checkForUserInput();
}

// ================================================================
//  COMMAND PARSER
// ================================================================
//void checkForUserInput()
//{
//  Serial2.printf("In checkForUserInput()");
//
//  if (!pStream->available())
//  {
//    return;
//  }
//
//  char cmd = pStream->read();
//  pStream->printf("Received: 0x%02X\n", cmd);
//
//  switch (cmd)
//  {
//  case 'D':
//  case 'd':
//  {
//    String arg = pStream->readStringUntil('\n');
//    arg.trim();
//
//    if (arg.startsWith(","))
//    {
//      float distMeters = arg.substring(1).toFloat();
//
//      if (distMeters == 0.0f)
//      {
//        pStream->println("Error: distance cannot be zero");
//        break;
//      }
//
//      long deltaTicks = (long)metersToTicks(distMeters);
//
//      odom.startAvgTicks = getAvgTicks();
//      odom.targetDeltaTicks = deltaTicks;
//      odom.forward = (distMeters > 0);
//      odom.active = true;
//
//      pStream->printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);
//
//      int leftPwm, rightPwm;
//      if (odom.forward)
//      {
//        leftPwm = BASE_PWM + PWM_OFFSET;
//        rightPwm = BASE_PWM - PWM_OFFSET;
//      }
//      else
//      {
//        leftPwm = BASE_PWM - PWM_OFFSET;
//        rightPwm = BASE_PWM + PWM_OFFSET;
//      }
//
//      drive(odom.forward ? leftPwm : -leftPwm,
//        odom.forward ? rightPwm : -rightPwm);
//    }
//    else
//    {
//      pStream->println("Invalid: expected D,<meters>");
//    }
//    break;
//  }
//
//  case 'S':
//  case 's':
//  {
//    pStream->println("Stop command received");
//    stopMotors();
//    odom.active = false;
//    while (1)
//    {
//      digitalWrite(LED_PIN, !digitalRead(LED_PIN));
//      delay(500);
//    }
//    break;
//  }
//  case 'U':
//  case 'u':
//  {
//    pStream->println(F("Start Program Update - Send new HEX file!"));
//
//    // exact string your TeensyOTA1.ttl macro expects
//    //pStream->println(F("waiting for hex lines..."));
//    Serial2.println(F("waiting for hex lines..."));
//
//    uint32_t buffer_addr, buffer_size;
//    if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
//    {
//      //pStream->println("Failed to init buffer");
//      Serial2.println("Failed to init buffer");
//      return;
//    }
//
//    // clear any leftover characters in the receive buffer
//    while (pStream->available())
//    {
//      //pStream->read();
//      Serial2.read();
//    }
//
//    update_firmware(&Serial2, &Serial2, buffer_addr, buffer_size);
//
//    firmware_buffer_free(buffer_addr, buffer_size);
//    REBOOT;
//    break;
//  }
//  }
//}

// ================================================================
//  COMMAND PARSER (USB + Bluetooth safe)
// ================================================================
// ================================================================
//  COMMAND PARSER (USB + Bluetooth safe)
// ================================================================
void checkForUserInput()
{
  // Check both ports so commands work from USB (when plugged in) OR Bluetooth
  if (Serial.available())   processCommand(Serial);
  if (Serial2.available())  processCommand(Serial2);
}

// Helper that does the actual work (keeps code DRY)
//void processCommand(Stream& serial_port)
//{
//  char cmd = serial_port.read();
//  serial_port.printf("Received: 0x%02X\n", cmd);
//
//  switch (cmd)
//  {
//  case 'D':
//  case 'd':
//  {
//    String arg = serial_port.readStringUntil('\n');
//    arg.trim();
//
//    if (arg.startsWith(","))
//    {
//      float distMeters = arg.substring(1).toFloat();
//
//      if (distMeters == 0.0f)
//      {
//        serial_port.println("Error: distance cannot be zero");
//        break;
//      }
//
//      long deltaTicks = (long)metersToTicks(distMeters);
//
//      odom.startAvgTicks = getAvgTicks();
//      odom.targetDeltaTicks = deltaTicks;
//      odom.forward = (distMeters > 0);
//      odom.active = true;
//
//      serial_port.printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);
//
//      int leftPwm, rightPwm;
//      if (odom.forward)
//      {
//        leftPwm = BASE_PWM + PWM_OFFSET;
//        rightPwm = BASE_PWM - PWM_OFFSET;
//      }
//      else
//      {
//        leftPwm = BASE_PWM - PWM_OFFSET;
//        rightPwm = BASE_PWM + PWM_OFFSET;
//      }
//
//      drive(odom.forward ? leftPwm : -leftPwm,
//        odom.forward ? rightPwm : -rightPwm);
//    }
//    else
//    {
//      serial_port.println("Invalid: expected D,<meters>");
//    }
//    break;
//  }
//
//  case 'R':
//  case 'r':
//  {
//    serial_port.println("Resetting Tick Counts");
//    stopMotors();
//    odom.active = false;
//    leftEnc.write(0);
//    rightEnc.write(0);
//
//    serial_port.printf("Tick Counts Now %lu, %lu\n", leftEnc.read(), rightEnc.read() );
//    break;
//  }
//
//  case 'S':
//  case 's':
//  {
//    serial_port.println("Stop command received");
//    stopMotors();
//    odom.active = false;
//    while (1)
//    {
//      digitalWrite(LED_PIN, !digitalRead(LED_PIN));
//      delay(500);
//    }
//    break;
//  }
//
//  case 'U':
//  case 'u':
//  {
//    // === FORCE OUTPUT TO SERIAL2 (Bluetooth) - critical for the macro ===
//    Serial2.println(F("Start Program Update - Send new HEX file!"));
//    Serial2.println(F("waiting for hex lines..."));
//
//    delay(50);   // give HC-05 time to transmit the message
//
//    uint32_t buffer_addr, buffer_size;
//    if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
//    {
//      Serial2.println("Failed to init buffer");
//      return;
//    }
//
//    // clear any leftover data in Bluetooth buffer
//    while (Serial2.available())
//    {
//      Serial2.read();
//    }
//
//    update_firmware(&Serial2, &Serial2, buffer_addr, buffer_size);
//    firmware_buffer_free(buffer_addr, buffer_size);
//    REBOOT;
//    break;
//  }
//  }
//}

// Helper that does the actual work (keeps code DRY)
//void processCommand(Stream& serial_port)
//{
//  if (!serial_port.available()) return;
//
//  char cmd = serial_port.read();
//  serial_port.printf("Received: 0x%02X ('%c')\n", cmd, cmd);
//
//  switch (cmd)
//  {
//  case 'D':
//  case 'd':
//  {
//    String arg = serial_port.readStringUntil('\n');
//    arg.trim();
//
//    if (arg.startsWith(","))
//    {
//      float distMeters = arg.substring(1).toFloat();
//
//      if (distMeters == 0.0f)
//      {
//        serial_port.println("Error: distance cannot be zero");
//        break;
//      }
//
//      long deltaTicks = (long)metersToTicks(distMeters);
//
//      odom.startAvgTicks = getAvgTicks();
//      odom.targetDeltaTicks = deltaTicks;
//      odom.forward = (distMeters > 0);
//      odom.active = true;
//
//      serial_port.printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);
//
//      int leftPwm, rightPwm;
//      if (odom.forward)
//      {
//        leftPwm = BASE_PWM + PWM_OFFSET;
//        rightPwm = BASE_PWM - PWM_OFFSET;
//      }
//      else
//      {
//        leftPwm = BASE_PWM - PWM_OFFSET;
//        rightPwm = BASE_PWM + PWM_OFFSET;
//      }
//
//      drive(odom.forward ? leftPwm : -leftPwm,
//        odom.forward ? rightPwm : -rightPwm);
//    }
//    else
//    {
//      serial_port.println("Invalid: expected D,<meters>");
//    }
//    break;
//  }
//
//  case 'R':
//  case 'r':
//  {
//    serial_port.println("Resetting Tick Counts");
//    stopMotors();
//    odom.active = false;
//    leftEnc.write(0);
//    rightEnc.write(0);
//
//    serial_port.printf("Tick Counts Now %ld, %ld\n", leftEnc.read(), rightEnc.read());
//    // consume any leftover newline/CR that the terminal may have sent
//    serial_port.readStringUntil('\n');
//    break;
//  }
//
//  case 'S':
//  case 's':
//  {
//    serial_port.println("Stop command received");
//    stopMotors();
//    odom.active = false;
//    while (1)
//    {
//      digitalWrite(LED_PIN, !digitalRead(LED_PIN));
//      delay(500);
//    }
//    break;
//  }
//
//  case 'U':
//  case 'u':
//  {
//    Serial2.println(F("Start Program Update - Send new HEX file!"));
//    Serial2.println(F("waiting for hex lines..."));
//
//    delay(50);
//
//    uint32_t buffer_addr, buffer_size;
//    if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
//    {
//      Serial2.println("Failed to init buffer");
//      return;
//    }
//
//    while (Serial2.available())
//    {
//      Serial2.read();
//    }
//
//    update_firmware(&Serial2, &Serial2, buffer_addr, buffer_size);
//    firmware_buffer_free(buffer_addr, buffer_size);
//    REBOOT;
//    break;
//  }
//
//  default:
//    // This will show you exactly what character arrived
//    serial_port.printf("Unknown command: 0x%02X ('%c') — ignoring\n", cmd, cmd);
//    // consume rest of line so leftover \r\n doesn't confuse future commands
//    serial_port.readStringUntil('\n');
//    break;
//  }
//}

// Helper that does the actual work (keeps code DRY)
void processCommand(Stream& serial_port)
{
  if (!serial_port.available()) return;

  // === Special fast-path for single-character OTA command (critical for macro) ===
  char firstChar = serial_port.peek();   // look without consuming
  if (firstChar == 'U' || firstChar == 'u')
  {
    serial_port.read();   // consume the 'U'
    serial_port.printf("Received: 0x%02X ('%c')\n", firstChar, firstChar);

    // === FORCE OUTPUT TO SERIAL2 (Bluetooth) - critical for the macro ===
    Serial2.println(F("Start Program Update - Send new HEX file!"));
    Serial2.println(F("waiting for hex lines..."));

    delay(50);   // give HC-05 time to transmit the message

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

    while (Serial2.available())
    {
      Serial2.read();
    }

    update_firmware(&Serial2, &Serial2, buffer_addr, buffer_size);
    firmware_buffer_free(buffer_addr, buffer_size);
    REBOOT;
    return;   // important - exit early
  }

  // === Normal line-based parsing for all other commands ===
  String line = serial_port.readStringUntil('\n');
  line.trim();

  if (line.length() == 0) return;

  serial_port.printf("Received command: '%s'\n", line.c_str());

  line.toUpperCase();

  if (line.startsWith("D,") || line.startsWith("DIST,"))
  {
    int commaIndex = line.indexOf(',');
    String numStr = line.substring(commaIndex + 1);
    float distMeters = numStr.toFloat();

    if (distMeters == 0.0f)
    {
      serial_port.println("Error: distance cannot be zero");
      return;
    }

    long deltaTicks = (long)metersToTicks(distMeters);

    odom.startAvgTicks = getAvgTicks();
    odom.targetDeltaTicks = deltaTicks;
    odom.forward = (distMeters > 0);
    odom.active = true;

    serial_port.printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);

    int leftPwm, rightPwm;
    if (odom.forward)
    {
      leftPwm = BASE_PWM + PWM_OFFSET;
      rightPwm = BASE_PWM - PWM_OFFSET;
    }
    else
    {
      leftPwm = BASE_PWM - PWM_OFFSET;
      rightPwm = BASE_PWM + PWM_OFFSET;
    }

    drive(odom.forward ? leftPwm : -leftPwm,
      odom.forward ? rightPwm : -rightPwm);
  }
  else if (line == "R" || line == "RESET")
  {
    serial_port.println("Resetting Tick Counts");
    stopMotors();
    odom.active = false;
    leftEnc.write(0);
    rightEnc.write(0);

    serial_port.printf("Tick Counts Now %ld, %ld\n", leftEnc.read(), rightEnc.read());
  }
  else if (line == "S" || line == "STOP")
  {
    serial_port.println("Stop command received");
    stopMotors();
    odom.active = false;
    //while (1)
    //{
    //  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    //  delay(500);
    //}
  }
  else
  {
    serial_port.printf("Unknown command: '%s'\n", line.c_str());
    serial_port.println("Valid commands: D,<meters>   RESET   STOP   OTA");
  }
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

Name: 2WheelEncoderNoPID_OTA_Refactored.ino

Author: Frank Paynter (with refinements)

Date: 2026-05-03

Teensy 4.1 + DRV8871 + Encoders + HC-05 OTA

Commands: D,<meters> (positive forward, negative reverse)

S (stop)

U (OTA update)

Notes:

05/04/26 added //#define NO_MOTORS so I can debug without motors running

//#define NO_MOTORS //added 05/05/26

// ================================================================

// PIN DEFINITIONS

// ================================================================

const uint8_t LEFT_ENC_A = 30;

const uint8_t LEFT_ENC_B = 29;

const uint8_t RIGHT_ENC_A = 32;

const uint8_t RIGHT_ENC_B = 31;

const uint8_t LEFT_IN1 = 11;

const uint8_t LEFT_IN2 = 12;

const uint8_t RIGHT_IN1 = 9;

const uint8_t RIGHT_IN2 = 10;

const uint8_t LED_PIN = 13;

// ================================================================

// TUNABLES

// ================================================================

const int BASE_PWM = 175;

const int PWM_OFFSET = 7;

const float WHEEL_RADIUS_M = 0.033f;

const float TICKS_PER_REV = 16.0f;

const unsigned long TELEMETRY_MS = 100;

// ================================================================

// INCLUDES AND GLOBALS

// ================================================================

#include <Arduino.h>

#include <Encoder.h>

#include "FXUtil.h"

extern "C"

{

#include "FlashTxx.h"

}

Encoder leftEnc(LEFT_ENC_A, LEFT_ENC_B);

Encoder rightEnc(RIGHT_ENC_A, RIGHT_ENC_B);

Stream* pStream = nullptr;

struct Odometry

{

long startAvgTicks = 0;

long targetDeltaTicks = 0;

bool active = false;

bool forward = true;

} odom;

unsigned long lastTelemetry = 0;

// ================================================================

// MOTOR CONTROL

// ================================================================

void drive(int leftPWM, int rightPWM)

{

#ifndef NO_MOTORS

if (leftPWM >= 0)

{

analogWrite(LEFT_IN1, leftPWM);

digitalWrite(LEFT_IN2, LOW);

}

else

{

analogWrite(LEFT_IN2, -leftPWM);

digitalWrite(LEFT_IN1, LOW);

}

if (rightPWM >= 0)

{

analogWrite(RIGHT_IN1, rightPWM);

digitalWrite(RIGHT_IN2, LOW);

}

else

{

analogWrite(RIGHT_IN2, -rightPWM);

digitalWrite(RIGHT_IN1, LOW);

}

#endif // !NO_MOTORS

}

void stopMotors()

{

//Serial2.println("In StopMotors()"); // keep your debug print

#ifndef NO_MOTORS

// === ROBUST STOP FOR DRV8871 + Teensy 4.1 ===

// 1. Explicitly kill any active PWM signals on ALL four pins

analogWrite(LEFT_IN1, 0);

analogWrite(LEFT_IN2, 0);

analogWrite(RIGHT_IN1, 0);

analogWrite(RIGHT_IN2, 0);

// 2. Force both inputs LOW on each motor → clean coast stop

digitalWrite(LEFT_IN1, LOW);

digitalWrite(LEFT_IN2, LOW);

digitalWrite(RIGHT_IN1, LOW);

digitalWrite(RIGHT_IN2, LOW);

#endif // !NO_MOTORS

}

// ================================================================

// ODOMETRY

// ================================================================

long getAvgTicks()

{

return (leftEnc.read() + rightEnc.read()) / 2;

}

float metersToTicks(float meters)

{

const float circ = 2.0f * 3.14159265f * WHEEL_RADIUS_M;

return meters * (TICKS_PER_REV / circ);

}

// ================================================================

// SETUP

// ================================================================

void setup()

{

Serial.begin(115200);

delay(2000);

Serial2.begin(115200);

while (!Serial2) {}

pStream = Serial ? (Stream*)&Serial : (Stream*)&Serial2;

pStream->println("\n=== 2-Wheel Encoder + OTA Ready ===");

pStream->println("Commands: D,<meters> S U");

pStream->println("Msec\tL_Tick\tR_Tick");

pinMode(LED_PIN, OUTPUT);

pinMode(LEFT_IN1, OUTPUT);

pinMode(LEFT_IN2, OUTPUT);

pinMode(RIGHT_IN1, OUTPUT);

pinMode(RIGHT_IN2, OUTPUT);

stopMotors();

}

// ================================================================

// LOOP

// ================================================================

void loop()

{

unsigned long now = millis();

if (now - lastTelemetry >= TELEMETRY_MS)

{

if (odom.active)

{

pStream->printf("%lu\t%ld\t%ld\n", now, leftEnc.read(), rightEnc.read());

}

lastTelemetry = now;

}

if (odom.active)

{

long progress = getAvgTicks() - odom.startAvgTicks;

bool reached = odom.forward ? (progress >= odom.targetDeltaTicks)

: (progress <= odom.targetDeltaTicks);

if (reached)

{

pStream->println("Target reached - stopping!");

stopMotors();

odom.active = false;

}

digitalWrite(LED_PIN, !digitalRead(LED_PIN));

checkForUserInput();

}

// ================================================================

// COMMAND PARSER

// ================================================================

//void checkForUserInput()

//{

// Serial2.printf("In checkForUserInput()");

// if (!pStream->available())

// {

// return;

// }

// char cmd = pStream->read();

// pStream->printf("Received: 0x%02X\n", cmd);

// switch (cmd)

// {

// case 'D':

// case 'd':

// {

// String arg = pStream->readStringUntil('\n');

// arg.trim();

// if (arg.startsWith(","))

// {

// float distMeters = arg.substring(1).toFloat();

// if (distMeters == 0.0f)

// {

// pStream->println("Error: distance cannot be zero");

// break;

// }

// long deltaTicks = (long)metersToTicks(distMeters);

// odom.startAvgTicks = getAvgTicks();

// odom.targetDeltaTicks = deltaTicks;

// odom.forward = (distMeters > 0);

// odom.active = true;

// pStream->printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);

// int leftPwm, rightPwm;

// if (odom.forward)

// {

// leftPwm = BASE_PWM + PWM_OFFSET;

// rightPwm = BASE_PWM - PWM_OFFSET;

// }

// else

// {

// leftPwm = BASE_PWM - PWM_OFFSET;

// rightPwm = BASE_PWM + PWM_OFFSET;

// }

// drive(odom.forward ? leftPwm : -leftPwm,

// odom.forward ? rightPwm : -rightPwm);

// }

// else

// {

// pStream->println("Invalid: expected D,<meters>");

// }

// break;

// }

// case 'S':

// case 's':

// {

// pStream->println("Stop command received");

// stopMotors();

// odom.active = false;

// while (1)

// {

// digitalWrite(LED_PIN, !digitalRead(LED_PIN));

// delay(500);

// }

// break;

// }

// case 'U':

// case 'u':

// {

// pStream->println(F("Start Program Update - Send new HEX file!"));

// // exact string your TeensyOTA1.ttl macro expects

// //pStream->println(F("waiting for hex lines..."));

// Serial2.println(F("waiting for hex lines..."));

// uint32_t buffer_addr, buffer_size;

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

// {

// //pStream->println("Failed to init buffer");

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

// return;

// }

// // clear any leftover characters in the receive buffer

// while (pStream->available())

// {

// //pStream->read();

// Serial2.read();

// }

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

// firmware_buffer_free(buffer_addr, buffer_size);

// REBOOT;

// break;

// }

//}

// ================================================================

// COMMAND PARSER (USB + Bluetooth safe)

// ================================================================

// COMMAND PARSER (USB + Bluetooth safe)

// ================================================================

void checkForUserInput()

{

// Check both ports so commands work from USB (when plugged in) OR Bluetooth

if (Serial.available()) processCommand(Serial);

if (Serial2.available()) processCommand(Serial2);

}

// Helper that does the actual work (keeps code DRY)

//void processCommand(Stream& serial_port)

//{

// char cmd = serial_port.read();

// serial_port.printf("Received: 0x%02X\n", cmd);

// switch (cmd)

// {

// case 'D':

// case 'd':

// {

// String arg = serial_port.readStringUntil('\n');

// arg.trim();

// if (arg.startsWith(","))

// {

// float distMeters = arg.substring(1).toFloat();

// if (distMeters == 0.0f)

// {

// serial_port.println("Error: distance cannot be zero");

// break;

// }

// long deltaTicks = (long)metersToTicks(distMeters);

// odom.startAvgTicks = getAvgTicks();

// odom.targetDeltaTicks = deltaTicks;

// odom.forward = (distMeters > 0);

// odom.active = true;

// serial_port.printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);

// int leftPwm, rightPwm;

// if (odom.forward)

// {

// leftPwm = BASE_PWM + PWM_OFFSET;

// rightPwm = BASE_PWM - PWM_OFFSET;

// }

// else

// {

// leftPwm = BASE_PWM - PWM_OFFSET;

// rightPwm = BASE_PWM + PWM_OFFSET;

// }

// drive(odom.forward ? leftPwm : -leftPwm,

// odom.forward ? rightPwm : -rightPwm);

// }

// else

// {

// serial_port.println("Invalid: expected D,<meters>");

// }

// break;

// }

// case 'R':

// case 'r':

// {

// serial_port.println("Resetting Tick Counts");

// stopMotors();

// odom.active = false;

// leftEnc.write(0);

// rightEnc.write(0);

// serial_port.printf("Tick Counts Now %lu, %lu\n", leftEnc.read(), rightEnc.read() );

// break;

// }

// case 'S':

// case 's':

// {

// serial_port.println("Stop command received");

// stopMotors();

// odom.active = false;

// while (1)

// {

// digitalWrite(LED_PIN, !digitalRead(LED_PIN));

// delay(500);

// }

// break;

// }

// case 'U':

// case 'u':

// {

// // === FORCE OUTPUT TO SERIAL2 (Bluetooth) - critical for the macro ===

// Serial2.println(F("Start Program Update - Send new HEX file!"));

// Serial2.println(F("waiting for hex lines..."));

// delay(50); // give HC-05 time to transmit the message

// uint32_t buffer_addr, buffer_size;

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

// {

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

// return;

// }

// // clear any leftover data in Bluetooth buffer

// while (Serial2.available())

// {

// Serial2.read();

// }

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

// firmware_buffer_free(buffer_addr, buffer_size);

// REBOOT;

// break;

// }

//}

// Helper that does the actual work (keeps code DRY)

//void processCommand(Stream& serial_port)

//{

// if (!serial_port.available()) return;

// char cmd = serial_port.read();

// serial_port.printf("Received: 0x%02X ('%c')\n", cmd, cmd);

// switch (cmd)

// {

// case 'D':

// case 'd':

// {

// String arg = serial_port.readStringUntil('\n');

// arg.trim();

// if (arg.startsWith(","))

// {

// float distMeters = arg.substring(1).toFloat();

// if (distMeters == 0.0f)

// {

// serial_port.println("Error: distance cannot be zero");

// break;

// }

// long deltaTicks = (long)metersToTicks(distMeters);

// odom.startAvgTicks = getAvgTicks();

// odom.targetDeltaTicks = deltaTicks;

// odom.forward = (distMeters > 0);

// odom.active = true;

// serial_port.printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);

// int leftPwm, rightPwm;

// if (odom.forward)

// {

// leftPwm = BASE_PWM + PWM_OFFSET;

// rightPwm = BASE_PWM - PWM_OFFSET;

// }

// else

// {

// leftPwm = BASE_PWM - PWM_OFFSET;

// rightPwm = BASE_PWM + PWM_OFFSET;

// }

// drive(odom.forward ? leftPwm : -leftPwm,

// odom.forward ? rightPwm : -rightPwm);

// }

// else

// {

// serial_port.println("Invalid: expected D,<meters>");

// }

// break;

// }

// case 'R':

// case 'r':

// {

// serial_port.println("Resetting Tick Counts");

// stopMotors();

// odom.active = false;

// leftEnc.write(0);

// rightEnc.write(0);

// serial_port.printf("Tick Counts Now %ld, %ld\n", leftEnc.read(), rightEnc.read());

// // consume any leftover newline/CR that the terminal may have sent

// serial_port.readStringUntil('\n');

// break;

// }

// case 'S':

// case 's':

// {

// serial_port.println("Stop command received");

// stopMotors();

// odom.active = false;

// while (1)

// {

// digitalWrite(LED_PIN, !digitalRead(LED_PIN));

// delay(500);

// }

// break;

// }

// case 'U':

// case 'u':

// {

// Serial2.println(F("Start Program Update - Send new HEX file!"));

// Serial2.println(F("waiting for hex lines..."));

// delay(50);

// uint32_t buffer_addr, buffer_size;

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

// {

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

// return;

// }

// while (Serial2.available())

// {

// Serial2.read();

// }

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

// firmware_buffer_free(buffer_addr, buffer_size);

// REBOOT;

// break;

// }

// default:

// // This will show you exactly what character arrived

// serial_port.printf("Unknown command: 0x%02X ('%c') — ignoring\n", cmd, cmd);

// // consume rest of line so leftover \r\n doesn't confuse future commands

// serial_port.readStringUntil('\n');

// break;

// }

//}

// Helper that does the actual work (keeps code DRY)

void processCommand(Stream& serial_port)

{

if (!serial_port.available()) return;

// === Special fast-path for single-character OTA command (critical for macro) ===

char firstChar = serial_port.peek(); // look without consuming

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

{

serial_port.read(); // consume the 'U'

serial_port.printf("Received: 0x%02X ('%c')\n", firstChar, firstChar);

// === FORCE OUTPUT TO SERIAL2 (Bluetooth) - critical for the macro ===

Serial2.println(F("Start Program Update - Send new HEX file!"));

Serial2.println(F("waiting for hex lines..."));

delay(50); // give HC-05 time to transmit the message

uint32_t buffer_addr, buffer_size;

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

{

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

return;

}

while (Serial2.available())

{

Serial2.read();

}

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

firmware_buffer_free(buffer_addr, buffer_size);

REBOOT;

return; // important - exit early

}

// === Normal line-based parsing for all other commands ===

String line = serial_port.readStringUntil('\n');

line.trim();

if (line.length() == 0) return;

serial_port.printf("Received command: '%s'\n", line.c_str());

line.toUpperCase();

if (line.startsWith("D,") || line.startsWith("DIST,"))

{

int commaIndex = line.indexOf(',');

String numStr = line.substring(commaIndex + 1);

float distMeters = numStr.toFloat();

if (distMeters == 0.0f)

{

serial_port.println("Error: distance cannot be zero");

return;

}

long deltaTicks = (long)metersToTicks(distMeters);

odom.startAvgTicks = getAvgTicks();

odom.targetDeltaTicks = deltaTicks;

odom.forward = (distMeters > 0);

odom.active = true;

serial_port.printf("Driving %.3f m (%ld ticks)\n", distMeters, deltaTicks);

int leftPwm, rightPwm;

if (odom.forward)

{

leftPwm = BASE_PWM + PWM_OFFSET;

rightPwm = BASE_PWM - PWM_OFFSET;

}

else

{

leftPwm = BASE_PWM - PWM_OFFSET;

rightPwm = BASE_PWM + PWM_OFFSET;

}

drive(odom.forward ? leftPwm : -leftPwm,

odom.forward ? rightPwm : -rightPwm);

}

else if (line == "R" || line == "RESET")

{

serial_port.println("Resetting Tick Counts");

stopMotors();

odom.active = false;

leftEnc.write(0);

rightEnc.write(0);

serial_port.printf("Tick Counts Now %ld, %ld\n", leftEnc.read(), rightEnc.read());

}

else if (line == "S" || line == "STOP")

{

serial_port.println("Stop command received");

stopMotors();

odom.active = false;

//while (1)

//{

// digitalWrite(LED_PIN, !digitalRead(LED_PIN));

// delay(500);

//}

}

else

{

serial_port.printf("Unknown command: '%s'\n", line.c_str());

serial_port.println("Valid commands: D,<meters> RESET STOP OTA");

}

The next big step in the Teensy code will be to integrate the MPU6050 6DoF gyro into the code so that the robot can make accurate PID-controlled specified-angle turns

Stay tuned,

Frank

Cheap Yellow Display – based Solfeggio Tone Generator

Leave a reply

Posted 21 January 2026

My wife recently asked me if I could build a ‘Solfeggio’ tone generator that she could listen to as a sleep aid. A quick net search yielded the following:

Solfeggio frequencies are a set of specific tones believed to promote healing, relaxation, and balance. The primary nine frequencies range from 174 Hz to 963 Hz, each associated with different benefits, such as emotional healing, stress reduction, and chakra alignment.

963 Hz: Connection to the divine and awakening.
These frequencies can be used in sound therapy and meditation practices to enhance well-being.

174 Hz: Pain relief and grounding.

285 Hz: Healing tissue and organs.

396 Hz: Liberating guilt and fear.

417 Hz: Facilitating change and undoing situations.

528 Hz: Transformation and DNA repair.

639 Hz: Enhancing communication and relationships.

741 Hz: Awakening intuition and problem-solving.

852 Hz: Returning to spiritual order.

As a result of a previous project to create a GPS-enabled DashCam Power Switch, I thought this would be a perfect ‘Cheap Yellow Display project. The Cheap Yellow Display (CYD) is a ESP32-2432S028R development board integrated with a 240×320 pixel color touch-sensitive display. In the case of my DashCam project, this meant that I only needed a GPS module, a low-Rds MOSFET, and a bit of circuitry to make it all work – neat. For this tone generation project, it looked like I might be able to do the whole thing with just a CWD and a pair of BT earbuds, as the ESP32-2432S028R dev board incorporates BT capability. The wife already had a pair of BT earbuds, and I had acquired two more CYD modules after it worked so well for my DashCam, so I was all set.

As I did with the DashCam project, I started consulting with Grok right away. If you haven’t yet taken advantage of this – you should – Not only can Grok code in any language, it is also mind-bogglingly fast for internet searches on particular topics. So I started out with this conversation:

Me: I want to build a rechargeable battery-operated tone generator using the ESP32/CYD configuration. I would like the battery to be the same (or smaller) width/length as the CYD. The input to the charging circuit would be +5V from the Type-C connector on the CYD. What battery and charging circuit would you suggest?

Grok: For a rechargeable battery-operated tone generator using the ESP32-based Cheap Yellow Display (CYD), which measures approximately 50mm x 86mm, you’ll want a compact 3.7V LiPo battery that fits within those dimensions for easy integration (e.g., mounted behind the board). The tone generator can use the ESP32’s built-in DAC (8-bit resolution) or PWM for audio output, connected to a small speaker or amp. Power management involves charging the battery via the CYD’s USB-C (which supplies 5V) and boosting the battery voltage to 5V for the board when unplugged, as the CYD expects 5V input on its power pins.Suggested BatteryA 3.7V 1000mAh LiPo battery with JST connector, such as a 603450 cell or equivalent.

Why this? It provides 3-5 hours of runtime for a low-power tone generator (ESP32 + display draws ~200-300mA at 5V under load; enable deep sleep modes to extend life). It’s widely available, rechargeable, and includes built-in protection (PCM) against overcharge/discharge. Smaller options like 300-500mAh (e.g., 402030 at 40mm x 20mm x 3mm) work if runtime isn’t critical, but 1000mAh balances size and capacity for ESP32 projects. ezsbc.shop +3 Avoid larger capacities (e.g., 3000mAh) if they exceed ~65mm x 35mm, as they may not fit neatly.

Dimensions: Approximately 60mm x 34mm x 5mm (fits well within 86mm x 50mm, leaving room for wiring).

Suggested Charging Circuit: Use a TP4056-based module with integrated battery protection (BMS).

Why this? It’s a simple, inexpensive ($1-2) 1A charger for single-cell LiPo batteries, accepting 5V input directly from the CYD’s USB-C (via wires to the module’s IN+ / IN- pads). It handles charging to 4.2V safely and includes overcharge/over-discharge protection. When USB-C is plugged in, it charges the battery; when unplugged, the module’s OUT+ / OUT- pads provide battery voltage (3.7V)
Additional component needed: Pair it with a small boost converter like the MT3608 (adjustable to 5V output) connected to the TP4056’s OUT pads. This boosts the 3.7V battery voltage to 5V for the CYD’s 5V pin (avoid direct 3.7V connection, as the board’s regulator expects 5V for stable operation). github.com +2 Add a switch on the boost output for power control.

There was some back-and-forth to nail down the actual modules I would need, but pretty soon I had the battery (li-ion 603450), charger (TP4056) and DC-DC converter (MT3608) on order from Amazon. The reason I went with separate charger and up-converter modules instead of the IP5306 all-in-one is due the IP5306 requiring a minimum load current to remain active, and this would conflict with my plan to put the ESP32 to sleep during idle times to extend battery life.

After getting the hardware issues resolved, the next step was the ESP32 software. I had Grok develop the software using Arduino code, with the following specs:

Me: Referencing our discussion of my ESP32 CYD Sinewave Tone Generator Project, I would like to see your take on Arduino code for the ESP32, with the following specs:

Non-K&R bracket convention (beginning bracket on new line)
Nine (9) touch-sensitive radio buttons for the ‘Solfeggio frequencies’ (https://www.bing.com/search?q=complete+list+of+solfeggio+frequencies&FORM=R5FD1).
A volume slider (horizontal at the bottom or vertical on the side)
Tone output directed to Bluetooth transmitter
Sleep mode after 10 minutes from coming out of sleep mode
Battery charge indicator based on A/D measurements of the LiPo battery voltage
Whatever other obvious requirement that I have failed to mention

Within a few seconds, Grok produced the following code:

// ESP32 CYD Sinewave Tone Generator for Solfeggio Frequencies
// Uses TFT_eSPI for display, XPT2046_Touchscreen for touch, arduino-audio-tools for audio generation and Bluetooth A2DP source
// Assumes TFT_eSPI User_Setup.h configured for CYD: ILI9341, pins as per Random Nerd Tutorials
// Bluetooth output to paired device (e.g., hearing aids or speaker)
// Battery voltage on GPIO35 with 2:1 divider (adjust calibration as needed)
// Deep sleep after 10 min inactivity, wake on touch IRQ (GPIO27 low)

// Libraries (install via Arduino Library Manager)
#include <TFT_eSPI.h>
#include <XPT2046_Touchscreen.h>
#include <AudioTools.h>
#include <BluetoothA2DPSource.h>

// Display and Touch
TFT_eSPI tft = TFT_eSPI();
#define XPT2046_IRQ 27
#define XPT2046_MOSI 23
#define XPT2046_MISO 19
#define XPT2046_SCK 18
#define XPT2046_CS 22
SPIClass touchscreenSPI = SPIClass(VSPI);
XPT2046_Touchscreen ts(XPT2046_CS, XPT2046_IRQ);

// Audio
BluetoothA2DPSource a2dp_source;
SineWaveGenerator<int16_t> sineWave;
GeneratedSoundStream<int16_t> soundStream(2, 44100);  // Stereo, 44.1kHz
AudioInfo audioInfo(44100, 2, 16);

// Solfeggio Frequencies (Hz)
const float frequencies[9] = {174, 285, 396, 417, 528, 639, 741, 852, 963};
int selectedFreqIndex = -1;  // None selected initially

// UI Dimensions (240x320 portrait)
const int screenWidth = 240;
const int screenHeight = 320;
const int buttonSize = 70;
const int buttonSpacing = 10;
const int sliderHeight = 30;
const int sliderWidth = screenWidth - 20;
const int sliderX = 10;
const int sliderY = screenHeight - sliderHeight - 10;

// Volume (0-127)
int volume = 64;

// Battery
const int batteryPin = 35;
const float minVoltage = 3.0;  // LiPo low
const float maxVoltage = 4.2;  // LiPo full
const float dividerRatio = 2.0;  // 2:1 divider

// Sleep
unsigned long lastActivityTime = 0;
const unsigned long sleepTimeout = 10UL * 60 * 1000;  // 10 minutes

void setup()
{
  Serial.begin(115200);

  // Display
  tft.init();
  tft.setRotation(0);  // Portrait
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(2);

  // Touch
  touchscreenSPI.begin(XPT2046_SCK, XPT2046_MISO, XPT2046_MOSI, XPT2046_CS);
  ts.begin(touchscreenSPI);
  ts.setRotation(0);

  // Bluetooth
  a2dp_source.start("CYD Tone Generator");  // Device name
  soundStream.begin(audioInfo);
  sineWave.begin(audioInfo, 0);  // Initial freq 0 (silent)

  // Audio pipeline
  a2dp_source.set_stream_reader(&soundStream);

  // Battery pin
  pinMode(batteryPin, INPUT);

  // Wake setup (for after deep sleep)
  esp_sleep_enable_gpio_wakeup();
  gpio_wakeup_enable((gpio_num_t)XPT2046_IRQ, GPIO_INTR_LOW_LEVEL);

  // Initial draw
  drawUI();
  updateBatteryIndicator();

  lastActivityTime = millis();
}

void loop()
{
  checkTouch();
  checkSleep();

  // Generate audio if frequency selected
  if (selectedFreqIndex != -1)
  {
    sineWave.setFrequency(frequencies[selectedFreqIndex]);
    sineWave.setAmplitude(volume / 127.0f);  // 0-1 range
    soundStream.add(sineWave);
  }
  else
  {
    sineWave.setFrequency(0);  // Silent
  }

  delay(10);  // Light loop
}

void drawUI()
{
  // Draw 3x3 buttons for frequencies
  int row = 0;
  int col = 0;
  for (int i = 0; i < 9; i++)
  {
    int x = buttonSpacing + col * (buttonSize + buttonSpacing);
    int y = buttonSpacing + row * (buttonSize + buttonSpacing);
    tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);
    tft.setCursor(x + 10, y + 25);
    tft.printf("%.0f", frequencies[i]);
    col++;
    if (col > 2)
    {
      col = 0;
      row++;
    }
  }

  // Volume slider
  tft.drawRect(sliderX, sliderY, sliderWidth, sliderHeight, TFT_WHITE);
  updateSlider();
  
  // Battery indicator (top right)
  tft.setCursor(screenWidth - 50, 10);
  tft.print("Bat:");
}

void updateSlider()
{
  int fillWidth = map(volume, 0, 127, 0, sliderWidth);
  tft.fillRect(sliderX + 1, sliderY + 1, fillWidth - 1, sliderHeight - 2, TFT_GREEN);
  tft.fillRect(sliderX + fillWidth, sliderY + 1, sliderWidth - fillWidth - 1, sliderHeight - 2, TFT_BLACK);
}

void updateBatteryIndicator()
{
  float adcValue = analogRead(batteryPin) / 4095.0 * 3.3 * dividerRatio;  // Calibrate as needed
  int percent = map(adcValue * 100, minVoltage * 100, maxVoltage * 100, 0, 100);
  percent = constrain(percent, 0, 100);
  
  tft.setCursor(screenWidth - 30, 10);
  tft.printf("%3d%%", percent);
}

void checkTouch()
{
  if (ts.tirqTouched() && ts.touched())
  {
    TS_Point p = ts.getPoint();
    // Calibrate touch (adjust based on your CYD, typical range)
    int x = map(p.x, 200, 3700, 0, screenWidth);
    int y = map(p.y, 200, 3700, 0, screenHeight);
    
    lastActivityTime = millis();

    // Check buttons
    int row = (y - buttonSpacing) / (buttonSize + buttonSpacing);
    int col = (x - buttonSpacing) / (buttonSize + buttonSpacing);
    if (row >= 0 && row < 3 && col >= 0 && col < 3)
    {
      int index = row * 3 + col;
      if (index != selectedFreqIndex)
      {
        selectedFreqIndex = index;
        // Redraw buttons to highlight selected (simple: text bold or color)
        redrawButtons();
      }
      else
      {
        selectedFreqIndex = -1;  // Toggle off
        redrawButtons();
      }
    }

    // Check slider
    if (y >= sliderY && y <= sliderY + sliderHeight && x >= sliderX && x <= sliderX + sliderWidth)
    {
      volume = map(x - sliderX, 0, sliderWidth, 0, 127);
      volume = constrain(volume, 0, 127);
      updateSlider();
    }
  }
}

void redrawButtons()
{
  int row = 0;
  int col = 0;
  for (int i = 0; i < 9; i++)
  {
    int x = buttonSpacing + col * (buttonSize + buttonSpacing);
    int y = buttonSpacing + row * (buttonSize + buttonSpacing);
    tft.fillRect(x + 1, y + 1, buttonSize - 2, buttonSize - 2, (i == selectedFreqIndex) ? TFT_BLUE : TFT_BLACK);
    tft.setCursor(x + 10, y + 25);
    tft.printf("%.0f", frequencies[i]);
    col++;
    if (col > 2)
    {
      col = 0;
      row++;
    }
  }
}

void checkSleep()
{
  if (millis() - lastActivityTime > sleepTimeout)
  {
    // Prepare for sleep
    tft.fillScreen(TFT_BLACK);  // Optional
    esp_deep_sleep_start();
  }
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

// ESP32 CYD Sinewave Tone Generator for Solfeggio Frequencies

// Uses TFT_eSPI for display, XPT2046_Touchscreen for touch, arduino-audio-tools for audio generation and Bluetooth A2DP source

// Assumes TFT_eSPI User_Setup.h configured for CYD: ILI9341, pins as per Random Nerd Tutorials

// Bluetooth output to paired device (e.g., hearing aids or speaker)

// Battery voltage on GPIO35 with 2:1 divider (adjust calibration as needed)

// Deep sleep after 10 min inactivity, wake on touch IRQ (GPIO27 low)

// Libraries (install via Arduino Library Manager)

#include <TFT_eSPI.h>

#include <XPT2046_Touchscreen.h>

#include <AudioTools.h>

#include <BluetoothA2DPSource.h>

// Display and Touch

TFT_eSPI tft = TFT_eSPI();

#define XPT2046_IRQ 27

#define XPT2046_MOSI 23

#define XPT2046_MISO 19

#define XPT2046_SCK 18

#define XPT2046_CS 22

SPIClass touchscreenSPI = SPIClass(VSPI);

XPT2046_Touchscreen ts(XPT2046_CS, XPT2046_IRQ);

// Audio

BluetoothA2DPSource a2dp_source;

SineWaveGenerator<int16_t> sineWave;

GeneratedSoundStream<int16_t> soundStream(2, 44100); // Stereo, 44.1kHz

AudioInfo audioInfo(44100, 2, 16);

// Solfeggio Frequencies (Hz)

const float frequencies[9] = {174, 285, 396, 417, 528, 639, 741, 852, 963};

int selectedFreqIndex = -1; // None selected initially

// UI Dimensions (240x320 portrait)

const int screenWidth = 240;

const int screenHeight = 320;

const int buttonSize = 70;

const int buttonSpacing = 10;

const int sliderHeight = 30;

const int sliderWidth = screenWidth - 20;

const int sliderX = 10;

const int sliderY = screenHeight - sliderHeight - 10;

// Volume (0-127)

int volume = 64;

// Battery

const int batteryPin = 35;

const float minVoltage = 3.0; // LiPo low

const float maxVoltage = 4.2; // LiPo full

const float dividerRatio = 2.0; // 2:1 divider

// Sleep

unsigned long lastActivityTime = 0;

const unsigned long sleepTimeout = 10UL * 60 * 1000; // 10 minutes

void setup()

{

Serial.begin(115200);

// Display

tft.init();

tft.setRotation(0); // Portrait

tft.fillScreen(TFT_BLACK);

tft.setTextColor(TFT_WHITE, TFT_BLACK);

tft.setTextSize(2);

// Touch

touchscreenSPI.begin(XPT2046_SCK, XPT2046_MISO, XPT2046_MOSI, XPT2046_CS);

ts.begin(touchscreenSPI);

ts.setRotation(0);

// Bluetooth

a2dp_source.start("CYD Tone Generator"); // Device name

soundStream.begin(audioInfo);

sineWave.begin(audioInfo, 0); // Initial freq 0 (silent)

// Audio pipeline

a2dp_source.set_stream_reader(&soundStream);

// Battery pin

pinMode(batteryPin, INPUT);

// Wake setup (for after deep sleep)

esp_sleep_enable_gpio_wakeup();

gpio_wakeup_enable((gpio_num_t)XPT2046_IRQ, GPIO_INTR_LOW_LEVEL);

// Initial draw

drawUI();

updateBatteryIndicator();

lastActivityTime = millis();

}

void loop()

{

checkTouch();

checkSleep();

// Generate audio if frequency selected

if (selectedFreqIndex != -1)

{

sineWave.setFrequency(frequencies[selectedFreqIndex]);

sineWave.setAmplitude(volume / 127.0f); // 0-1 range

soundStream.add(sineWave);

}

else

{

sineWave.setFrequency(0); // Silent

}

delay(10); // Light loop

}

void drawUI()

{

// Draw 3x3 buttons for frequencies

int row = 0;

int col = 0;

for (int i = 0; i < 9; i++)

{

int x = buttonSpacing + col * (buttonSize + buttonSpacing);

int y = buttonSpacing + row * (buttonSize + buttonSpacing);

tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);

tft.setCursor(x + 10, y + 25);

tft.printf("%.0f", frequencies[i]);

col++;

if (col > 2)

{

col = 0;

row++;

}

// Volume slider

tft.drawRect(sliderX, sliderY, sliderWidth, sliderHeight, TFT_WHITE);

updateSlider();

// Battery indicator (top right)

tft.setCursor(screenWidth - 50, 10);

tft.print("Bat:");

}

void updateSlider()

{

int fillWidth = map(volume, 0, 127, 0, sliderWidth);

tft.fillRect(sliderX + 1, sliderY + 1, fillWidth - 1, sliderHeight - 2, TFT_GREEN);

tft.fillRect(sliderX + fillWidth, sliderY + 1, sliderWidth - fillWidth - 1, sliderHeight - 2, TFT_BLACK);

}

void updateBatteryIndicator()

{

float adcValue = analogRead(batteryPin) / 4095.0 * 3.3 * dividerRatio; // Calibrate as needed

int percent = map(adcValue * 100, minVoltage * 100, maxVoltage * 100, 0, 100);

percent = constrain(percent, 0, 100);

tft.setCursor(screenWidth - 30, 10);

tft.printf("%3d%%", percent);

}

void checkTouch()

{

if (ts.tirqTouched() && ts.touched())

{

TS_Point p = ts.getPoint();

// Calibrate touch (adjust based on your CYD, typical range)

int x = map(p.x, 200, 3700, 0, screenWidth);

int y = map(p.y, 200, 3700, 0, screenHeight);

lastActivityTime = millis();

// Check buttons

int row = (y - buttonSpacing) / (buttonSize + buttonSpacing);

int col = (x - buttonSpacing) / (buttonSize + buttonSpacing);

if (row >= 0 && row < 3 && col >= 0 && col < 3)

{

int index = row * 3 + col;

if (index != selectedFreqIndex)

{

selectedFreqIndex = index;

// Redraw buttons to highlight selected (simple: text bold or color)

redrawButtons();

}

else

{

selectedFreqIndex = -1; // Toggle off

redrawButtons();

}

// Check slider

if (y >= sliderY && y <= sliderY + sliderHeight && x >= sliderX && x <= sliderX + sliderWidth)

{

volume = map(x - sliderX, 0, sliderWidth, 0, 127);

volume = constrain(volume, 0, 127);

updateSlider();

}

void redrawButtons()

{

int row = 0;

int col = 0;

for (int i = 0; i < 9; i++)

{

int x = buttonSpacing + col * (buttonSize + buttonSpacing);

int y = buttonSpacing + row * (buttonSize + buttonSpacing);

tft.fillRect(x + 1, y + 1, buttonSize - 2, buttonSize - 2, (i == selectedFreqIndex) ? TFT_BLUE : TFT_BLACK);

tft.setCursor(x + 10, y + 25);

tft.printf("%.0f", frequencies[i]);

col++;

if (col > 2)

{

col = 0;

row++;

}

void checkSleep()

{

if (millis() - lastActivityTime > sleepTimeout)

{

// Prepare for sleep

tft.fillScreen(TFT_BLACK); // Optional

esp_deep_sleep_start();

}

Not only did Grok produce initial code, but it helped me work through some subtle issues with the ‘AudioTools’ library as I had not used this library before. Grok would offer code, then I would run (or try to run) and report back on any errors encountered. Pretty soon I had code that compiled cleanly and ran, and then it was a case of exercising the code and reporting any debug output back to Grok. This went around for a while, but we eventually got to working code. I learned that while Grok is very fast, he’s not always all that accurate. For anything other than the most trivial programming task, the human at the other end of the conversation must have relevant experience. At one point in the debugging process, Grok was recommending some hardware changes on the CYD board, but I knew from prior experience with the DashCam project that no such mods were required for detecting user touches on the touch-screen. So I was able to provide Grok with a small working program to do this and Grok was happy to incorporate the new information into the complete program. This, however, brought up another ‘gotcha’. Grok takes input from the human, thinks for a few seconds, and then disgorges another complete program – without any accompanying explanation. So, I said “what was the problem with the previous code?” and got a pretty comprehensive answer- yay.

Grok really doesn’t do a very good job of troubleshooting or code debug at all – that’s where the human’s engineering experience comes in. At one point I tried to describe the ‘cut the world in two’ troubleshooting technique I have been using for decades, but I’m not sure if Grok has any ability to remember things like that, or any ability to apply them, so I suspect that was a waste of time. Grok will continue to produce code without explanation if you let it – kind of a ‘random walk’ shotgun approach AFAICT. At the end of this hours-long session, I got kind of frustrated with this behavior and terminated the session. I put up a DM to @xAI, but never got a response.

Eventually, over a 2-3 day period we got a working program that would connect via BT to my wife’s earbuds and play the tone (with some ‘reverb’ added, even!) selected by tapping a button on the CYD screen.

// ESP32 CYD Sinewave Tone Generator for Solfeggio Frequencies
// Uses TFT_eSPI for display, XPT2046_Touchscreen for touch, I2S for audio output to PCM5102
// Assumes TFT_eSPI User_Setup.h configured for CYD: ILI9341, pins as per Random Nerd Tutorials
// Wired output to PCM5102 DAC (pins: BCK=22, WS=27, DATA=26)
// Deep sleep after 20 min inactivity, wake on touch IRQ (GPIO36); state persisted in NVS

// Libraries (install via Arduino Library Manager)
#include <TFT_eSPI.h>
#include <XPT2046_Touchscreen.h>
#include <driver/i2s.h>
#include <math.h>  // For sin and PI
#include <Preferences.h>  // For NVS storage

// Touch pin definitions (VSPI with custom pins)
#define TOUCH_CS 33
#define TOUCH_IRQ 36
#define TOUCH_PRESSURE_THRESHOLD 100  // Minimum z (pressure) to consider a valid touch (filter noise)
#define TOUCH_PRESS_HIGH 150  // Higher threshold for initial press detection
#define TOUCH_PRESS_LOW 50    // Lower threshold for release detection (hysteresis)
#define TOUCH_CLK 25
#define TOUCH_MISO 39
#define TOUCH_MOSI 32

// Touch calibration constants
const int TS_MINX = 3831, TS_MINY = 308, TS_MAXX = 294, TS_MAXY = 3798;

// Display and Touch
TFT_eSPI tft = TFT_eSPI();
SPIClass touchscreenSPI = SPIClass(VSPI);  // Use VSPI bus
XPT2046_Touchscreen ts(TOUCH_CS, TOUCH_IRQ);

// I2S pin configuration
#define I2S_BCK_PIN 22   // Bit clock
#define I2S_WS_PIN 27    // Word select (LRCK)
#define I2S_DATA_PIN 26  // Data out (DIN)

// Audio parameters
#define SAMPLE_RATE 44100
#define BITS_PER_SAMPLE 16
#define CHANNELS 2  // Stereo
#define BUFFER_SIZE 1024  // Increased for smoother playback (adjust if needed)

// Solfeggio Frequencies (Hz)
const float frequencies[9] = { 174, 285, 396, 417, 528, 639, 741, 852, 963 };
int selectedFreqIndex = -1;  // None selected initially

// UI Dimensions (240x320 portrait)
const int screenWidth = 240;
const int screenHeight = 320;
const int buttonSize = 70;
const int buttonSpacing = 10;
const int sliderHeight = 30;  // Increased for finger usability
const int sliderSpacing = 5;
const int sliderWidth = screenWidth - 20;
const int sliderX = 10;
const int leftVolSliderY = 245;  // Start after buttons (end ~240)
const int rightVolSliderY = leftVolSliderY + sliderHeight + sliderSpacing;

// Audio controls
float leftVol = 0.5f;  // Left ear volume (0-1)
float rightVol = 0.5f;  // Right ear volume (0-1)

// Battery indicator
const int batteryPin = 35;
const float minVoltage = 3.0;  // LiPo low
const float maxVoltage = 4.2;  // LiPo full
const float dividerRatio = 2.0;  // 2:1 divider
int batRectWidth = 30;
int batRectHeight = 5;

// Deep sleep (after inactivity)
unsigned long lastActivityTime = 0;
const unsigned long sleepTimeout = 20UL * 60 * 1000;  // 20 minutes

// RTC data for surviving deep sleep
RTC_DATA_ATTR int bootCount = 0;

// Global buffer for samples
int16_t audioBuffer[BUFFER_SIZE * CHANNELS];

// NVS for state persistence
Preferences prefs;

// Touch state for hysteresis
bool isPressed = false;

// Function to generate audio data (sine wave with separate L/R volumes)
void generateAudioBuffer(float frequency)
{
  static float phase = 0.0;
  const float phaseIncrement = 2.0 * M_PI * frequency / SAMPLE_RATE;

  for (int i = 0; i < BUFFER_SIZE; i++)
  {
    int16_t sample = (int16_t)(sin(phase) * 32767.0);  // Raw sine wave full amplitude

    // Apply separate volumes to L/R
    audioBuffer[2 * i] = (int16_t)(sample * leftVol);      // Left channel
    audioBuffer[2 * i + 1] = (int16_t)(sample * rightVol);  // Right channel

    phase += phaseIncrement;
    if (phase >= 2.0 * M_PI) phase -= 2.0 * M_PI;
  }
}

// Method to print the reason by which ESP32 has been awaken from sleep
void print_wakeup_reason()
{
  esp_sleep_wakeup_cause_t wakeup_reason;

  wakeup_reason = esp_sleep_get_wakeup_cause();

  switch (wakeup_reason)
  {
  case ESP_SLEEP_WAKEUP_EXT0: Serial.println("Wakeup caused by external signal using RTC_IO"); break;
  case ESP_SLEEP_WAKEUP_EXT1: Serial.println("Wakeup caused by external signal using RTC_CNTL"); break;
  case ESP_SLEEP_WAKEUP_TIMER: Serial.println("Wakeup caused by timer"); break;
  case ESP_SLEEP_WAKEUP_TOUCHPAD: Serial.println("Wakeup caused by touchpad"); break;
  case ESP_SLEEP_WAKEUP_ULP: Serial.println("Wakeup caused by ULP program"); break;
  default: Serial.printf("Wakeup was not caused by deep sleep: %d\n", wakeup_reason); break;
  }
}

void setup()
{
  Serial.begin(115200);
  touchscreenSPI.begin(TOUCH_CLK, TOUCH_MISO, TOUCH_MOSI, TOUCH_CS);
  ts.begin(touchscreenSPI);
  ts.setRotation(1);

  tft.init();
  tft.invertDisplay(1);
  tft.setRotation(2);
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(2);

  pinMode(batteryPin, INPUT);

  // Increment boot number and print it every reboot
  ++bootCount;
  Serial.println("Boot number: " + String(bootCount));

  // Print the wakeup reason for ESP32
  print_wakeup_reason();

  // Load state from NVS
  prefs.begin("cyd_tone", false);
  selectedFreqIndex = prefs.getInt("freqIndex", -1);
  leftVol = prefs.getFloat("leftVol", 0.5f);
  rightVol = prefs.getFloat("rightVol", 0.5f);
  prefs.end();

  // Configure I2S
  i2s_config_t i2s_config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),
    .sample_rate = SAMPLE_RATE,
    .bits_per_sample = (i2s_bits_per_sample_t)BITS_PER_SAMPLE,
    .channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,
    .communication_format = I2S_COMM_FORMAT_STAND_I2S,
    .intr_alloc_flags = 0,
    .dma_buf_count = 8,
    .dma_buf_len = BUFFER_SIZE,
    .use_apll = true,  // Better clock accuracy for audio
    .tx_desc_auto_clear = true,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_config = {
    .bck_io_num = I2S_BCK_PIN,
    .ws_io_num = I2S_WS_PIN,
    .data_out_num = I2S_DATA_PIN,
    .data_in_num = I2S_PIN_NO_CHANGE
  };

  // Install and start I2S driver
  if (i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL) != ESP_OK)
  {
    Serial.println("I2S driver install failed");
    while (1);
  }
  if (i2s_set_pin(I2S_NUM_0, &pin_config) != ESP_OK)
  {
    Serial.println("I2S pin set failed");
    while (1);
  }

  Serial.println("I2S initialized.");

  // Configure external wake up on GPIO36 (TOUCH_IRQ, wakes on low level when touched)
  esp_sleep_enable_ext0_wakeup((gpio_num_t)TOUCH_IRQ, 0);  // 0 for low level
  Serial.println("[DEBUG] Wake setup done");

  drawUI();
  Serial.printf("In setup(): Just before call to updateBatteryIndicator()\n");

  updateBatteryIndicator();
  lastActivityTime = millis();
}

void loop()
{
  // Prioritize audio
  float currentFreq = (selectedFreqIndex >= 0) ? frequencies[selectedFreqIndex] : 0.0f;
  generateAudioBuffer(currentFreq);

  // Write buffer to I2S
  size_t bytesWritten;
  i2s_write(I2S_NUM_0, audioBuffer, sizeof(audioBuffer), &bytesWritten, portMAX_DELAY);

  if (bytesWritten != sizeof(audioBuffer))
  {
    Serial.println("I2S write incomplete");
  }

  checkTouch();  // Check touch after audio to minimize interruption

  updateBatteryIndicator();

  if (millis() - lastActivityTime > sleepTimeout)
  {
    // Save state to NVS before sleep
    prefs.begin("cyd_tone", false);
    prefs.putInt("freqIndex", selectedFreqIndex);
    prefs.putFloat("leftVol", leftVol);
    prefs.putFloat("rightVol", rightVol);
    prefs.end();

    tft.fillScreen(TFT_BLACK);  // Optional clear before sleep
    Serial.println("Going to deep sleep now");
    esp_deep_sleep_start();  // Deep sleep; resets on wake
  }
}

// drawUI function
void drawUI()
{
  tft.fillScreen(TFT_BLACK);  // Clear screen

  // Draw 3x3 buttons for frequencies
  int row = 0;
  int col = 0;
  for (int i = 0; i < 9; i++)
  {
    int x = buttonSpacing + col * (buttonSize + buttonSpacing);
    int y = buttonSpacing + row * (buttonSize + buttonSpacing);
    tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);
    tft.setCursor(x + 10, y + 25);
    tft.printf("%.0f", frequencies[i]);
    col++;
    if (col > 2)
    {
      col = 0;
      row++;
    }
  }

  // Left Volume slider (cyan) with label
  tft.setTextSize(1);
  tft.setCursor(sliderX - 30, leftVolSliderY + (sliderHeight / 2) - 4);
  tft.print("L Vol");
  tft.setTextSize(2);
  tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);
  int leftFillWidth = static_cast<int>(leftVol * (sliderWidth - 2));
  tft.fillRect(sliderX + 1, leftVolSliderY + 1, leftFillWidth, sliderHeight - 2, TFT_CYAN);
  tft.fillRect(sliderX + 1 + leftFillWidth, leftVolSliderY + 1, sliderWidth - 2 - leftFillWidth, sliderHeight - 2, TFT_BLACK);

  // Right Volume slider (green) with label
  tft.setTextSize(1);
  tft.setCursor(sliderX - 30, rightVolSliderY + (sliderHeight / 2) - 4);
  tft.print("R Vol");
  tft.setTextSize(2);
  tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);
  int rightFillWidth = static_cast<int>(rightVol * (sliderWidth - 2));
  tft.fillRect(sliderX + 1, rightVolSliderY + 1, rightFillWidth, sliderHeight - 2, TFT_GREEN);
  tft.fillRect(sliderX + 1 + rightFillWidth, rightVolSliderY + 1, sliderWidth - 2 - rightFillWidth, sliderHeight - 2, TFT_BLACK);

  tft.drawRect(0, 0, batRectWidth, batRectHeight, TFT_WHITE);

  redrawButtons();  // Apply selected state
}

void updateBatteryIndicator()
{
  Serial.printf("In updateBatteryIndicator()\n");
  int adcCount = analogRead(batteryPin);
  float adcValue = adcCount / 4095.0 * 3.3 * dividerRatio;  // Calibrate as needed
  int percent = map(adcValue * 100, minVoltage * 100, maxVoltage * 100, 0, 100);
  percent = constrain(percent, 0, 100);
  int fillWidth = percent * batRectWidth / 100;
  int start_y = 10;

  //tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_BLACK);  // Clear previous
  tft.fillRect(0, start_y, batRectWidth, batRectHeight, TFT_BLACK);  // Clear previous

  if (percent <= 20)
  {
    tft.fillRect(0, start_y, fillWidth, batRectHeight, TFT_RED);
  }
  else if (percent > 20 && percent <= 50)
  {
    tft.fillRect(0, start_y, fillWidth, batRectHeight, TFT_ORANGE);
  }
  else if (percent > 50)
  {
    tft.fillRect(0, start_y, fillWidth, batRectHeight, TFT_GREEN);
  }

  tft.drawRect(0, start_y, batRectWidth, batRectHeight, TFT_WHITE);
}

// checkTouch function with hysteresis and state management
void checkTouch()
{
  TS_Point p = ts.getPoint();

  if (!isPressed)
  {
    if (p.z > TOUCH_PRESS_HIGH)
    {
      isPressed = true;
      processTouch(p, true);  // Initial press = true
    }
  }
  else
  {
    if (p.z < TOUCH_PRESS_LOW)
    {
      isPressed = false;
      // Process touch end if needed (not for this app)
    }
    else
    {
      processTouch(p, false);  // Drag, initial press = false
    }
  }
  delay(20);  // Reduced debounce for responsiveness
}

// Helper to process touch (called on press start or drag)
void processTouch(TS_Point p, bool initialPress)
{
  int x = map(p.y, TS_MINY, TS_MAXY, 0, screenWidth);  // Inverted X
  int y = map(p.x, TS_MINX, TS_MAXX, 0, screenHeight);  // Inverted Y
  Serial.printf("Mapped X: %d, Y: %d, Z: %d\n", x, y, p.z);

  // Check buttons first (only on initial press)
  if (initialPress)
  {
    int row = (y - buttonSpacing) / (buttonSize + buttonSpacing);
    int col = (x - buttonSpacing) / (buttonSize + buttonSpacing);
    if (row >= 0 && row < 3 && col >= 0 && col < 3)
    {
      int index = row * 3 + col;
      Serial.printf("Button pressed: %d\n", index);  // Debug
      if (index != selectedFreqIndex)
      {
        selectedFreqIndex = index;
      }
      else
      {
        selectedFreqIndex = -1;  // Toggle off
      }
      redrawButtons();
      return;  // Exit to avoid checking sliders
    }
  }

  // Check left volume slider (allow drag, initial or ongoing)
  if (y >= leftVolSliderY - 5 && y <= leftVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)
  {
    leftVol = static_cast<float>(x - sliderX) / sliderWidth;
    leftVol = constrain(leftVol, 0.0f, 1.0f);
    Serial.printf("Left Vol adjusted to: %.2f\n", leftVol);  // Debug
    int fillWidth = static_cast<int>(leftVol * (sliderWidth - 2));
    tft.fillRect(sliderX + 1, leftVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_CYAN);
    tft.fillRect(sliderX + 1 + fillWidth, leftVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);
    tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);  // Redraw border
  }

  // Check right volume slider (allow drag, initial or ongoing)
  if (y >= rightVolSliderY - 5 && y <= rightVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)
  {
    rightVol = static_cast<float>(x - sliderX) / sliderWidth;
    rightVol = constrain(rightVol, 0.0f, 1.0f);
    Serial.printf("Right Vol adjusted to: %.2f\n", rightVol);  // Debug
    int fillWidth = static_cast<int>(rightVol * (sliderWidth - 2));
    tft.fillRect(sliderX + 1, rightVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_GREEN);
    tft.fillRect(sliderX + 1 + fillWidth, rightVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);
    tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);  // Redraw border
  }

  lastActivityTime = millis();  // Reset inactivity timer on touch
}

// redrawButtons function
void redrawButtons()
{
  int row = 0;
  int col = 0;
  for (int i = 0; i < 9; i++)
  {
    int x = buttonSpacing + col * (buttonSize + buttonSpacing);
    int y = buttonSpacing + row * (buttonSize + buttonSpacing);
    tft.fillRect(x + 1, y + 1, buttonSize - 2, buttonSize - 2, (i == selectedFreqIndex) ? TFT_BLUE : TFT_BLACK);
    tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);  // Ensure border
    tft.setCursor(x + 10, y + 25);
    tft.printf("%.0f", frequencies[i]);
    col++;
    if (col > 2)
    {
      col = 0;
      row++;
    }
  }
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

// ESP32 CYD Sinewave Tone Generator for Solfeggio Frequencies

// Uses TFT_eSPI for display, XPT2046_Touchscreen for touch, I2S for audio output to PCM5102

// Assumes TFT_eSPI User_Setup.h configured for CYD: ILI9341, pins as per Random Nerd Tutorials

// Wired output to PCM5102 DAC (pins: BCK=22, WS=27, DATA=26)

// Deep sleep after 20 min inactivity, wake on touch IRQ (GPIO36); state persisted in NVS

// Libraries (install via Arduino Library Manager)

#include <TFT_eSPI.h>

#include <XPT2046_Touchscreen.h>

#include <driver/i2s.h>

#include <math.h> // For sin and PI

#include <Preferences.h> // For NVS storage

// Touch pin definitions (VSPI with custom pins)

#define TOUCH_CS 33

#define TOUCH_IRQ 36

#define TOUCH_PRESSURE_THRESHOLD 100 // Minimum z (pressure) to consider a valid touch (filter noise)

#define TOUCH_PRESS_HIGH 150 // Higher threshold for initial press detection

#define TOUCH_PRESS_LOW 50 // Lower threshold for release detection (hysteresis)

#define TOUCH_CLK 25

#define TOUCH_MISO 39

#define TOUCH_MOSI 32

// Touch calibration constants

const int TS_MINX = 3831, TS_MINY = 308, TS_MAXX = 294, TS_MAXY = 3798;

// Display and Touch

TFT_eSPI tft = TFT_eSPI();

SPIClass touchscreenSPI = SPIClass(VSPI); // Use VSPI bus

XPT2046_Touchscreen ts(TOUCH_CS, TOUCH_IRQ);

// I2S pin configuration

#define I2S_BCK_PIN 22 // Bit clock

#define I2S_WS_PIN 27 // Word select (LRCK)

#define I2S_DATA_PIN 26 // Data out (DIN)

// Audio parameters

#define SAMPLE_RATE 44100

#define BITS_PER_SAMPLE 16

#define CHANNELS 2 // Stereo

#define BUFFER_SIZE 1024 // Increased for smoother playback (adjust if needed)

// Solfeggio Frequencies (Hz)

const float frequencies[9] = { 174, 285, 396, 417, 528, 639, 741, 852, 963 };

int selectedFreqIndex = -1; // None selected initially

// UI Dimensions (240x320 portrait)

const int screenWidth = 240;

const int screenHeight = 320;

const int buttonSize = 70;

const int buttonSpacing = 10;

const int sliderHeight = 30; // Increased for finger usability

const int sliderSpacing = 5;

const int sliderWidth = screenWidth - 20;

const int sliderX = 10;

const int leftVolSliderY = 245; // Start after buttons (end ~240)

const int rightVolSliderY = leftVolSliderY + sliderHeight + sliderSpacing;

// Audio controls

float leftVol = 0.5f; // Left ear volume (0-1)

float rightVol = 0.5f; // Right ear volume (0-1)

// Battery indicator

const int batteryPin = 35;

const float minVoltage = 3.0; // LiPo low

const float maxVoltage = 4.2; // LiPo full

const float dividerRatio = 2.0; // 2:1 divider

int batRectWidth = 30;

int batRectHeight = 5;

// Deep sleep (after inactivity)

unsigned long lastActivityTime = 0;

const unsigned long sleepTimeout = 20UL * 60 * 1000; // 20 minutes

// RTC data for surviving deep sleep

RTC_DATA_ATTR int bootCount = 0;

// Global buffer for samples

int16_t audioBuffer[BUFFER_SIZE * CHANNELS];

// NVS for state persistence

Preferences prefs;

// Touch state for hysteresis

bool isPressed = false;

// Function to generate audio data (sine wave with separate L/R volumes)

void generateAudioBuffer(float frequency)

{

static float phase = 0.0;

const float phaseIncrement = 2.0 * M_PI * frequency / SAMPLE_RATE;

for (int i = 0; i < BUFFER_SIZE; i++)

{

int16_t sample = (int16_t)(sin(phase) * 32767.0); // Raw sine wave full amplitude

// Apply separate volumes to L/R

audioBuffer[2 * i] = (int16_t)(sample * leftVol); // Left channel

audioBuffer[2 * i + 1] = (int16_t)(sample * rightVol); // Right channel

phase += phaseIncrement;

if (phase >= 2.0 * M_PI) phase -= 2.0 * M_PI;

}

// Method to print the reason by which ESP32 has been awaken from sleep

void print_wakeup_reason()

{

esp_sleep_wakeup_cause_t wakeup_reason;

wakeup_reason = esp_sleep_get_wakeup_cause();

switch (wakeup_reason)

{

case ESP_SLEEP_WAKEUP_EXT0: Serial.println("Wakeup caused by external signal using RTC_IO"); break;

case ESP_SLEEP_WAKEUP_EXT1: Serial.println("Wakeup caused by external signal using RTC_CNTL"); break;

case ESP_SLEEP_WAKEUP_TIMER: Serial.println("Wakeup caused by timer"); break;

case ESP_SLEEP_WAKEUP_TOUCHPAD: Serial.println("Wakeup caused by touchpad"); break;

case ESP_SLEEP_WAKEUP_ULP: Serial.println("Wakeup caused by ULP program"); break;

default: Serial.printf("Wakeup was not caused by deep sleep: %d\n", wakeup_reason); break;

}

void setup()

{

Serial.begin(115200);

touchscreenSPI.begin(TOUCH_CLK, TOUCH_MISO, TOUCH_MOSI, TOUCH_CS);

ts.begin(touchscreenSPI);

ts.setRotation(1);

tft.init();

tft.invertDisplay(1);

tft.setRotation(2);

tft.fillScreen(TFT_BLACK);

tft.setTextColor(TFT_WHITE, TFT_BLACK);

tft.setTextSize(2);

pinMode(batteryPin, INPUT);

// Increment boot number and print it every reboot

++bootCount;

Serial.println("Boot number: " + String(bootCount));

// Print the wakeup reason for ESP32

print_wakeup_reason();

// Load state from NVS

prefs.begin("cyd_tone", false);

selectedFreqIndex = prefs.getInt("freqIndex", -1);

leftVol = prefs.getFloat("leftVol", 0.5f);

rightVol = prefs.getFloat("rightVol", 0.5f);

prefs.end();

// Configure I2S

i2s_config_t i2s_config = {

.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),

.sample_rate = SAMPLE_RATE,

.bits_per_sample = (i2s_bits_per_sample_t)BITS_PER_SAMPLE,

.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,

.communication_format = I2S_COMM_FORMAT_STAND_I2S,

.intr_alloc_flags = 0,

.dma_buf_count = 8,

.dma_buf_len = BUFFER_SIZE,

.use_apll = true, // Better clock accuracy for audio

.tx_desc_auto_clear = true,

.fixed_mclk = 0

};

i2s_pin_config_t pin_config = {

.bck_io_num = I2S_BCK_PIN,

.ws_io_num = I2S_WS_PIN,

.data_out_num = I2S_DATA_PIN,

.data_in_num = I2S_PIN_NO_CHANGE

};

// Install and start I2S driver

if (i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL) != ESP_OK)

{

Serial.println("I2S driver install failed");

while (1);

}

if (i2s_set_pin(I2S_NUM_0, &pin_config) != ESP_OK)

{

Serial.println("I2S pin set failed");

while (1);

}

Serial.println("I2S initialized.");

// Configure external wake up on GPIO36 (TOUCH_IRQ, wakes on low level when touched)

esp_sleep_enable_ext0_wakeup((gpio_num_t)TOUCH_IRQ, 0); // 0 for low level

Serial.println("[DEBUG] Wake setup done");

drawUI();

Serial.printf("In setup(): Just before call to updateBatteryIndicator()\n");

updateBatteryIndicator();

lastActivityTime = millis();

}

void loop()

{

// Prioritize audio

float currentFreq = (selectedFreqIndex >= 0) ? frequencies[selectedFreqIndex] : 0.0f;

generateAudioBuffer(currentFreq);

// Write buffer to I2S

size_t bytesWritten;

i2s_write(I2S_NUM_0, audioBuffer, sizeof(audioBuffer), &bytesWritten, portMAX_DELAY);

if (bytesWritten != sizeof(audioBuffer))

{

Serial.println("I2S write incomplete");

}

checkTouch(); // Check touch after audio to minimize interruption

updateBatteryIndicator();

if (millis() - lastActivityTime > sleepTimeout)

{

// Save state to NVS before sleep

prefs.begin("cyd_tone", false);

prefs.putInt("freqIndex", selectedFreqIndex);

prefs.putFloat("leftVol", leftVol);

prefs.putFloat("rightVol", rightVol);

prefs.end();

tft.fillScreen(TFT_BLACK); // Optional clear before sleep

Serial.println("Going to deep sleep now");

esp_deep_sleep_start(); // Deep sleep; resets on wake

}

// drawUI function

void drawUI()

{

tft.fillScreen(TFT_BLACK); // Clear screen

// Draw 3x3 buttons for frequencies

int row = 0;

int col = 0;

for (int i = 0; i < 9; i++)

{

int x = buttonSpacing + col * (buttonSize + buttonSpacing);

int y = buttonSpacing + row * (buttonSize + buttonSpacing);

tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);

tft.setCursor(x + 10, y + 25);

tft.printf("%.0f", frequencies[i]);

col++;

if (col > 2)

{

col = 0;

row++;

}

// Left Volume slider (cyan) with label

tft.setTextSize(1);

tft.setCursor(sliderX - 30, leftVolSliderY + (sliderHeight / 2) - 4);

tft.print("L Vol");

tft.setTextSize(2);

tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);

int leftFillWidth = static_cast<int>(leftVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, leftVolSliderY + 1, leftFillWidth, sliderHeight - 2, TFT_CYAN);

tft.fillRect(sliderX + 1 + leftFillWidth, leftVolSliderY + 1, sliderWidth - 2 - leftFillWidth, sliderHeight - 2, TFT_BLACK);

// Right Volume slider (green) with label

tft.setTextSize(1);

tft.setCursor(sliderX - 30, rightVolSliderY + (sliderHeight / 2) - 4);

tft.print("R Vol");

tft.setTextSize(2);

tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);

int rightFillWidth = static_cast<int>(rightVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, rightVolSliderY + 1, rightFillWidth, sliderHeight - 2, TFT_GREEN);

tft.fillRect(sliderX + 1 + rightFillWidth, rightVolSliderY + 1, sliderWidth - 2 - rightFillWidth, sliderHeight - 2, TFT_BLACK);

tft.drawRect(0, 0, batRectWidth, batRectHeight, TFT_WHITE);

redrawButtons(); // Apply selected state

}

void updateBatteryIndicator()

{

Serial.printf("In updateBatteryIndicator()\n");

int adcCount = analogRead(batteryPin);

float adcValue = adcCount / 4095.0 * 3.3 * dividerRatio; // Calibrate as needed

int percent = map(adcValue * 100, minVoltage * 100, maxVoltage * 100, 0, 100);

percent = constrain(percent, 0, 100);

int fillWidth = percent * batRectWidth / 100;

int start_y = 10;

//tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_BLACK); // Clear previous

tft.fillRect(0, start_y, batRectWidth, batRectHeight, TFT_BLACK); // Clear previous

if (percent <= 20)

{

tft.fillRect(0, start_y, fillWidth, batRectHeight, TFT_RED);

}

else if (percent > 20 && percent <= 50)

{

tft.fillRect(0, start_y, fillWidth, batRectHeight, TFT_ORANGE);

}

else if (percent > 50)

{

tft.fillRect(0, start_y, fillWidth, batRectHeight, TFT_GREEN);

}

tft.drawRect(0, start_y, batRectWidth, batRectHeight, TFT_WHITE);

}

// checkTouch function with hysteresis and state management

void checkTouch()

{

TS_Point p = ts.getPoint();

if (!isPressed)

{

if (p.z > TOUCH_PRESS_HIGH)

{

isPressed = true;

processTouch(p, true); // Initial press = true

}

else

{

if (p.z < TOUCH_PRESS_LOW)

{

isPressed = false;

// Process touch end if needed (not for this app)

}

else

{

processTouch(p, false); // Drag, initial press = false

}

delay(20); // Reduced debounce for responsiveness

}

// Helper to process touch (called on press start or drag)

void processTouch(TS_Point p, bool initialPress)

{

int x = map(p.y, TS_MINY, TS_MAXY, 0, screenWidth); // Inverted X

int y = map(p.x, TS_MINX, TS_MAXX, 0, screenHeight); // Inverted Y

Serial.printf("Mapped X: %d, Y: %d, Z: %d\n", x, y, p.z);

// Check buttons first (only on initial press)

if (initialPress)

{

int row = (y - buttonSpacing) / (buttonSize + buttonSpacing);

int col = (x - buttonSpacing) / (buttonSize + buttonSpacing);

if (row >= 0 && row < 3 && col >= 0 && col < 3)

{

int index = row * 3 + col;

Serial.printf("Button pressed: %d\n", index); // Debug

if (index != selectedFreqIndex)

{

selectedFreqIndex = index;

}

else

{

selectedFreqIndex = -1; // Toggle off

}

redrawButtons();

return; // Exit to avoid checking sliders

}

// Check left volume slider (allow drag, initial or ongoing)

if (y >= leftVolSliderY - 5 && y <= leftVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)

{

leftVol = static_cast<float>(x - sliderX) / sliderWidth;

leftVol = constrain(leftVol, 0.0f, 1.0f);

Serial.printf("Left Vol adjusted to: %.2f\n", leftVol); // Debug

int fillWidth = static_cast<int>(leftVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, leftVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_CYAN);

tft.fillRect(sliderX + 1 + fillWidth, leftVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);

tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE); // Redraw border

}

// Check right volume slider (allow drag, initial or ongoing)

if (y >= rightVolSliderY - 5 && y <= rightVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)

{

rightVol = static_cast<float>(x - sliderX) / sliderWidth;

rightVol = constrain(rightVol, 0.0f, 1.0f);

Serial.printf("Right Vol adjusted to: %.2f\n", rightVol); // Debug

int fillWidth = static_cast<int>(rightVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, rightVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_GREEN);

tft.fillRect(sliderX + 1 + fillWidth, rightVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);

tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE); // Redraw border

}

lastActivityTime = millis(); // Reset inactivity timer on touch

}

// redrawButtons function

void redrawButtons()

{

int row = 0;

int col = 0;

for (int i = 0; i < 9; i++)

{

int x = buttonSpacing + col * (buttonSize + buttonSpacing);

int y = buttonSpacing + row * (buttonSize + buttonSpacing);

tft.fillRect(x + 1, y + 1, buttonSize - 2, buttonSize - 2, (i == selectedFreqIndex) ? TFT_BLUE : TFT_BLACK);

tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE); // Ensure border

tft.setCursor(x + 10, y + 25);

tft.printf("%.0f", frequencies[i]);

col++;

if (col > 2)

{

col = 0;

row++;

}

This program worked fine, but had a few problems:

The ESP32 ‘deep sleep’ mode was problematic. The mode itself worked fine (and dropped the current drain down to just a few mA, but when the system came back awake, it wouldn’t reconnect to the BT earbuds. We eventually wound up using the ‘light sleep’ mode, which would reliably reconnect to the earbuds, but drew significantly more current (less battery life).
The earbuds themselves were an issue. When/if the system failed to reconnect, then only way to do so was to put the earbuds back in their charging container and force them back into ‘pairing’ mode so the tone generator box could ‘see’ them again, and this was a PITA.

These issues led my wife to ask if I could change the project to use wired earbuds rather than wireless. This made the design both simpler and more complex. Simpler because now ‘sleep’ wouldn’t be an issue as the wired earbuds eliminated the need to connect via BT. More complex, because the CYD has no native stereo output, so an additional module to convert I2S protocol stereo audio data to human-audible stereo sound was required, and in turn this required finding a third output-enabled GPIO line (the CYD configuration uses most of the ESP32’s GPIO lines to run the display), and this required cutting a board trace and running a jumper to access GPIO26.

Here’s my statement to Grok:

Me: referencing our CYD Tone Generator project, I want to modify it to use plug-in wired earbuds rather than BT. I believe this will require outputting the audio on two GPIO lines (22 & 27)? and implementing an audio amplifier and a volume control. Can you recommend a good audio amplifier package for this?

Grok immediately identified a really nice module:

Grok: I recommend the PCM5102A I2S DAC module (often labeled as “PCM5102 DAC Decoder” or similar). It’s a compact, affordable breakout board widely used in ESP32 audio projects for its simplicity and good sound quality (32-bit, up to 384kHz sampling). Many versions include a built-in 3.5mm headphone jack, making it plug-and-play for wired earbuds without additional soldering for the connector.

Why this one?

Directly interfaces with ESP32 I2S output (no need for the internal DAC on GPIO 25/26).

Provides line-level analog output that can drive most earbuds (16–32Ω impedance) at reasonable volumes without distortion—users commonly report success with direct connection for low-power applications like tones.

Low noise, high SNR (112dB), and minimal external components needed.

Cheap and available: ~$5–10 on Amazon, AliExpress, or electronics sites like Adafruit/SparkFun (search for “PCM5102 I2S DAC module”).

If your earbuds are very sensitive or you need more headroom, some variants include an integrated headphone amplifier (e.g., based on LM4880M—look for “PCM5102 with headphone amp” on Tindie or AliExpress for ~$10–15).

Connections (assuming standard I2S remapping in code):

VCC: 3.3V from ESP32.

GND: Shared ground.

BCLK (bit clock): Connect to your GPIO 22 (or remap as needed).

LRCK/WS (word select/left-right clock): You’ll likely need a third GPIO (e.g., 25 or 26) for this—add it if not already in your setup.

DIN/SDIN (data in): Connect to your GPIO 27.

FMT, FLT, DMP, SCL: Ground these (or leave default for standard I2S format).

XMT/XSMT: 3.3V (unmutes the output).

Analog out: Directly to 3.5mm jack for earbuds.

So I got a couple of the PCM5102A I2S DAC modules from Amazon, and wired them up according to Grok’s instructions. 3.3V, Gnd, BCLK, DIN all came from CN1 on the CYD, but I had to cut a trace to free GPIO26 from it’s normal connection through an audio amp to the ‘speaker’ connector so I could use it for LRCK. See below for the details:

Here’s the program:

/*
    Name:       CYD_ToneGen_DeepSleep_LibConflict_Reverb.ino
    Created:	1/20/2026 4:30:19 PM
    Author:     FRANK_XPS_9530\Frank
*/

// ESP32 CYD Sinewave Tone Generator for Solfeggio Frequencies
// Uses TFT_eSPI for display, XPT2046_Touchscreen for touch, I2S for audio output to PCM5102
// Assumes TFT_eSPI User_Setup.h configured for CYD: ILI9341, pins as per Random Nerd Tutorials
// Wired output to PCM5102 DAC (pins: BCK=22, WS=27, DATA=26)
// Deep sleep after 20 min inactivity, wake on touch IRQ (GPIO36); state persisted in NVS

// Libraries (install via Arduino Library Manager)
#include <TFT_eSPI.h>
#include <XPT2046_Touchscreen.h>
#include <driver/i2s.h>
#include <math.h>  // For sin and PI
#include <Preferences.h>  // For NVS storage

// Touch pin definitions (VSPI with custom pins)
#define TOUCH_CS 33
#define TOUCH_IRQ 36
#define TOUCH_PRESSURE_THRESHOLD 100  // Minimum z (pressure) to consider a valid touch (filter noise)
#define TOUCH_PRESS_HIGH 150  // Higher threshold for initial press detection
#define TOUCH_PRESS_LOW 50    // Lower threshold for release detection (hysteresis)
#define TOUCH_CLK 25
#define TOUCH_MISO 39
#define TOUCH_MOSI 32

// Touch calibration constants
const int TS_MINX = 3831, TS_MINY = 308, TS_MAXX = 294, TS_MAXY = 3798;

// Display and Touch
TFT_eSPI tft = TFT_eSPI();
SPIClass touchscreenSPI = SPIClass(VSPI);  // Use VSPI bus
XPT2046_Touchscreen ts(TOUCH_CS, TOUCH_IRQ);

// I2S pin configuration
#define I2S_BCK_PIN 22   // Bit clock
#define I2S_WS_PIN 27    // Word select (LRCK)
#define I2S_DATA_PIN 26  // Data out (DIN)

// Audio parameters
#define SAMPLE_RATE 44100
#define BITS_PER_SAMPLE 16
#define CHANNELS 2  // Stereo
#define BUFFER_SIZE 1024  // Increased for smoother playback (adjust if needed)
const float masterGain = 0.5f;  //01/20/26 Jo Pref: 0.5 = half previous max volume; tune to 0.4, 0.6, etc.

//reverb constants
#define DELAY_SAMPLES 22050  // ~0.5 s delay at 44.1 kHz
int16_t delayBuffer[DELAY_SAMPLES] = { 0 };
int delayPtr = 0;
const float reverbDecay = 0.4f;   // 0.0–1.0 → how fast echo fades
const float reverbFeedback = 0.35f; // 0.0–0.6 → how much echo feeds back (keep low to avoid ringing)

// Solfeggio Frequencies (Hz)
const float frequencies[9] = { 174, 285, 396, 417, 528, 639, 741, 852, 963 };
int selectedFreqIndex = -1;  // None selected initially

// UI Dimensions (240x320 portrait)
const int screenWidth = 240;
const int screenHeight = 320;
const int buttonSize = 70;
const int buttonSpacing = 10;
const int sliderHeight = 25;  // Reduced to fit on screen
const int sliderSpacing = 10;  // Adjusted spacing
const int sliderWidth = screenWidth - 20;
const int sliderX = 10;
const int leftVolSliderY = 255;  // Adjusted to match touch top y~260, avoid button overlap
const int rightVolSliderY = leftVolSliderY + sliderHeight + sliderSpacing;  // 255+25+10=290 to 315 (fits within 320)

// Audio controls
float leftVol = 0.5f;  // Left ear volume (0-1)
float rightVol = 0.5f;  // Right ear volume (0-1)

// Battery indicator (digital)
const int batteryPin = 35;
int batRectWidth = 30;
int batRectHeight = 5;

// Deep sleep (after inactivity)
unsigned long lastActivityTime = 0;
const unsigned long sleepTimeout = 20UL * 60 * 1000;  // 20 minutes

// RTC data for surviving deep sleep
RTC_DATA_ATTR int bootCount = 0;

// Global buffer for samples
int16_t audioBuffer[BUFFER_SIZE * CHANNELS];

// NVS for state persistence
Preferences prefs;

// Touch state for hysteresis
bool isPressed = false;

 //Function to generate audio data (sine wave with separate L/R volumes)
//01/20/26 mod to halve volume for Jo pref
void generateAudioBuffer(float frequency)
{
  static float phase = 0.0;
  const float phaseIncrement = 2.0 * M_PI * frequency / SAMPLE_RATE;

  for (int i = 0; i < BUFFER_SIZE; i++)
  {
    int16_t sample = (int16_t)(sin(phase) * 32767.0);  // full-scale dry sine

    // Apply L/R volumes first (slider 0–1)
    int32_t left = (int32_t)(sample * leftVol);
    int32_t right = (int32_t)(sample * rightVol);

    // Apply master gain (halves overall level without affecting slider mapping)
    left = (int32_t)(left * masterGain);
    right = (int32_t)(right * masterGain);

    // Clip to 16-bit range
    audioBuffer[2 * i] = (int16_t)constrain(left, INT16_MIN, INT16_MAX);
    audioBuffer[2 * i + 1] = (int16_t)constrain(right, INT16_MIN, INT16_MAX);

    phase += phaseIncrement;
    if (phase >= 2.0 * M_PI) phase -= 2.0 * M_PI;
  }
}

// Method to print the reason by which ESP32 has been awaken from sleep
void print_wakeup_reason()
{
  esp_sleep_wakeup_cause_t wakeup_reason;

  wakeup_reason = esp_sleep_get_wakeup_cause();

  switch (wakeup_reason)
  {
  case ESP_SLEEP_WAKEUP_EXT0: Serial.println("Wakeup caused by external signal using RTC_IO"); break;
  case ESP_SLEEP_WAKEUP_EXT1: Serial.println("Wakeup caused by external signal using RTC_CNTL"); break;
  case ESP_SLEEP_WAKEUP_TIMER: Serial.println("Wakeup caused by timer"); break;
  case ESP_SLEEP_WAKEUP_TOUCHPAD: Serial.println("Wakeup caused by touchpad"); break;
  case ESP_SLEEP_WAKEUP_ULP: Serial.println("Wakeup caused by ULP program"); break;
  default: Serial.printf("Wakeup was not caused by deep sleep: %d\n", wakeup_reason); break;
  }
}

void setup()
{
  Serial.begin(115200);
  touchscreenSPI.begin(TOUCH_CLK, TOUCH_MISO, TOUCH_MOSI, TOUCH_CS);
  ts.begin(touchscreenSPI);
  ts.setRotation(1);

  tft.init();
  tft.invertDisplay(1);
  tft.setRotation(2);
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(2);

  pinMode(batteryPin, INPUT);

  // Increment boot number and print it every reboot
  ++bootCount;
  Serial.println("Boot number: " + String(bootCount));

  // Print the wakeup reason for ESP32
  print_wakeup_reason();

  // Load state from NVS
  prefs.begin("cyd_tone", false);
  selectedFreqIndex = prefs.getInt("freqIndex", -1);
  //leftVol = prefs.getFloat("leftVol", 0.5f);
  //rightVol = prefs.getFloat("rightVol", 0.5f);
  leftVol = prefs.getFloat("leftVol", 0.1f);//Jo pref
  rightVol = prefs.getFloat("rightVol", 0.1f);//Jo pref
  prefs.end();

  // Configure I2S
  i2s_config_t i2s_config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),
    .sample_rate = SAMPLE_RATE,
    .bits_per_sample = (i2s_bits_per_sample_t)BITS_PER_SAMPLE,
    .channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,
    .communication_format = I2S_COMM_FORMAT_STAND_I2S,
    .intr_alloc_flags = 0,
    .dma_buf_count = 8,
    .dma_buf_len = BUFFER_SIZE,
    .use_apll = true,  // Better clock accuracy for audio
    .tx_desc_auto_clear = true,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_config = {
    .bck_io_num = I2S_BCK_PIN,
    .ws_io_num = I2S_WS_PIN,
    .data_out_num = I2S_DATA_PIN,
    .data_in_num = I2S_PIN_NO_CHANGE
  };

  // Install and start I2S driver
  if (i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL) != ESP_OK)
  {
    Serial.println("I2S driver install failed");
    while (1);
  }
  if (i2s_set_pin(I2S_NUM_0, &pin_config) != ESP_OK)
  {
    Serial.println("I2S pin set failed");
    while (1);
  }

  Serial.println("I2S initialized.");

  // Configure external wake up on GPIO36 (TOUCH_IRQ, wakes on low level when touched)
  esp_sleep_enable_ext0_wakeup((gpio_num_t)TOUCH_IRQ, 0);  // 0 for low level
  Serial.println("[DEBUG] Wake setup done");

  drawUI();
  updateBatteryIndicator();
  lastActivityTime = millis();
}

void loop()
{
  // Prioritize audio
  float currentFreq = (selectedFreqIndex >= 0) ? frequencies[selectedFreqIndex] : 0.0f;
  generateAudioBuffer(currentFreq);

  // Write buffer to I2S
  size_t bytesWritten;
  i2s_write(I2S_NUM_0, audioBuffer, sizeof(audioBuffer), &bytesWritten, portMAX_DELAY);

  if (bytesWritten != sizeof(audioBuffer))
  {
    Serial.println("I2S write incomplete");
  }

  checkTouch();  // Check touch after audio to minimize interruption

  updateBatteryIndicator();

  if (millis() - lastActivityTime > sleepTimeout)
  {
    // Save state to NVS before sleep
    prefs.begin("cyd_tone", false);
    prefs.putInt("freqIndex", selectedFreqIndex);
    prefs.putFloat("leftVol", leftVol);
    prefs.putFloat("rightVol", rightVol);
    prefs.end();

    tft.fillScreen(TFT_BLACK);  // Optional clear before sleep
    Serial.println("Going to deep sleep now");
    esp_deep_sleep_start();  // Deep sleep; resets on wake
  }
}

// drawUI function
void drawUI()
{
  tft.fillScreen(TFT_BLACK);  // Clear screen

  // Draw 3x3 buttons for frequencies
  int row = 0;
  int col = 0;
  for (int i = 0; i < 9; i++)
  {
    int x = buttonSpacing + col * (buttonSize + buttonSpacing);
    int y = buttonSpacing + row * (buttonSize + buttonSpacing);
    tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);
    tft.setCursor(x + 10, y + 25);
    tft.printf("%.0f", frequencies[i]);
    col++;
    if (col > 2)
    {
      col = 0;
      row++;
    }
  }

  // Left Volume slider (cyan) with label
  tft.setTextSize(1);
  tft.setCursor(sliderX - 30, leftVolSliderY + (sliderHeight / 2) - 4);
  tft.print("L");
  tft.setTextSize(2);
  tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);
  int leftFillWidth = static_cast<int>(leftVol * (sliderWidth - 2));
  tft.fillRect(sliderX + 1, leftVolSliderY + 1, leftFillWidth, sliderHeight - 2, TFT_CYAN);
  tft.fillRect(sliderX + 1 + leftFillWidth, leftVolSliderY + 1, sliderWidth - 2 - leftFillWidth, sliderHeight - 2, TFT_BLACK);

  // Right Volume slider (green) with label
  tft.setTextSize(1);
  tft.setCursor(sliderX - 30, rightVolSliderY + (sliderHeight / 2) - 4);
  tft.print("R");
  tft.setTextSize(2);
  tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);
  int rightFillWidth = static_cast<int>(rightVol * (sliderWidth - 2));
  tft.fillRect(sliderX + 1, rightVolSliderY + 1, rightFillWidth, sliderHeight - 2, TFT_GREEN);
  tft.fillRect(sliderX + 1 + rightFillWidth, rightVolSliderY + 1, sliderWidth - 2 - rightFillWidth, sliderHeight - 2, TFT_BLACK);

  tft.drawRect(0, 0, batRectWidth, batRectHeight, TFT_WHITE);

  redrawButtons();  // Apply selected state
}

// updateBatteryIndicator function using digitalRead
void updateBatteryIndicator()
{
  int batLevel = digitalRead(batteryPin);  // 1 = HIGH (good), 0 = LOW (low)
  //Serial.println("Battery digital: " + String(batLevel));  // Debug

  tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_BLACK); // Clear previous

  if (batLevel == 0)
  {
    tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_RED);  // Low
  }
  else
  {
    tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_GREEN);  // Good
  }

  tft.drawRect(0, 0, batRectWidth, batRectHeight, TFT_WHITE);
}

// checkTouch function with hysteresis and state management
void checkTouch()
{
  TS_Point p = ts.getPoint();

  if (!isPressed)
  {
    if (p.z > TOUCH_PRESS_HIGH)
    {
      isPressed = true;
      processTouch(p, true);  // Initial press = true
    }
  }
  else
  {
    if (p.z < TOUCH_PRESS_LOW)
    {
      isPressed = false;
      // Process touch end if needed (not for this app)
    }
    else
    {
      processTouch(p, false);  // Drag, initial press = false
    }
  }
  delay(20);  // Reduced debounce for responsiveness
}

// Helper to process touch (called on press start or drag)
void processTouch(TS_Point p, bool initialPress)
{
  int x = map(p.y, TS_MINY, TS_MAXY, 0, screenWidth);  // Inverted X
  int y = map(p.x, TS_MINX, TS_MAXX, 0, screenHeight);  // Inverted Y
  //Serial.printf("Mapped X: %d, Y: %d, Z: %d\n", x, y, p.z);

  // Check buttons first (only on initial press)
  if (initialPress)
  {
    int row = (y - buttonSpacing) / (buttonSize + buttonSpacing);
    int col = (x - buttonSpacing) / (buttonSize + buttonSpacing);
    if (row >= 0 && row < 3 && col >= 0 && col < 3)
    {
      int index = row * 3 + col;
      //Serial.printf("Button pressed: %d\n", index);  // Debug
      if (index != selectedFreqIndex)
      {
        selectedFreqIndex = index;
      }
      else
      {
        selectedFreqIndex = -1;  // Toggle off
      }
      redrawButtons();
      return;  // Exit to avoid checking sliders
    }
  }

  // Check left volume slider (allow drag, initial or ongoing)
  if (y >= leftVolSliderY - 5 && y <= leftVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)
  {
    leftVol = static_cast<float>(x - sliderX) / sliderWidth;
    leftVol = constrain(leftVol, 0.0f, 1.0f);
    //Serial.printf("Left Vol adjusted to: %.2f\n", leftVol);  // Debug
    int fillWidth = static_cast<int>(leftVol * (sliderWidth - 2));
    tft.fillRect(sliderX + 1, leftVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_CYAN);
    tft.fillRect(sliderX + 1 + fillWidth, leftVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);
    tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);  // Redraw border
  }

  // Check right volume slider (allow drag, initial or ongoing)
  if (y >= rightVolSliderY - 5 && y <= rightVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)
  {
    rightVol = static_cast<float>(x - sliderX) / sliderWidth;
    rightVol = constrain(rightVol, 0.0f, 1.0f);
    int fillWidth = static_cast<int>(rightVol * (sliderWidth - 2));
    tft.fillRect(sliderX + 1, rightVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_GREEN);
    tft.fillRect(sliderX + 1 + fillWidth, rightVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);
    tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);  // Redraw border
  }

  lastActivityTime = millis();  // Reset inactivity timer on touch
}

// redrawButtons function
void redrawButtons()
{
  int row = 0;
  int col = 0;
  for (int i = 0; i < 9; i++)
  {
    int x = buttonSpacing + col * (buttonSize + buttonSpacing);
    int y = buttonSpacing + row * (buttonSize + buttonSpacing);
    tft.fillRect(x + 1, y + 1, buttonSize - 2, buttonSize - 2, (i == selectedFreqIndex) ? TFT_BLUE : TFT_BLACK);
    tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);  // Ensure border
    tft.setCursor(x + 10, y + 25);
    tft.printf("%.0f", frequencies[i]);
    col++;
    if (col > 2)
    {
      col = 0;
      row++;
    }
  }
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

Name: CYD_ToneGen_DeepSleep_LibConflict_Reverb.ino

Created: 1/20/2026 4:30:19 PM

Author: FRANK_XPS_9530\Frank

// ESP32 CYD Sinewave Tone Generator for Solfeggio Frequencies

// Uses TFT_eSPI for display, XPT2046_Touchscreen for touch, I2S for audio output to PCM5102

// Assumes TFT_eSPI User_Setup.h configured for CYD: ILI9341, pins as per Random Nerd Tutorials

// Wired output to PCM5102 DAC (pins: BCK=22, WS=27, DATA=26)

// Deep sleep after 20 min inactivity, wake on touch IRQ (GPIO36); state persisted in NVS

// Libraries (install via Arduino Library Manager)

#include <TFT_eSPI.h>

#include <XPT2046_Touchscreen.h>

#include <driver/i2s.h>

#include <math.h> // For sin and PI

#include <Preferences.h> // For NVS storage

// Touch pin definitions (VSPI with custom pins)

#define TOUCH_CS 33

#define TOUCH_IRQ 36

#define TOUCH_PRESSURE_THRESHOLD 100 // Minimum z (pressure) to consider a valid touch (filter noise)

#define TOUCH_PRESS_HIGH 150 // Higher threshold for initial press detection

#define TOUCH_PRESS_LOW 50 // Lower threshold for release detection (hysteresis)

#define TOUCH_CLK 25

#define TOUCH_MISO 39

#define TOUCH_MOSI 32

// Touch calibration constants

const int TS_MINX = 3831, TS_MINY = 308, TS_MAXX = 294, TS_MAXY = 3798;

// Display and Touch

TFT_eSPI tft = TFT_eSPI();

SPIClass touchscreenSPI = SPIClass(VSPI); // Use VSPI bus

XPT2046_Touchscreen ts(TOUCH_CS, TOUCH_IRQ);

// I2S pin configuration

#define I2S_BCK_PIN 22 // Bit clock

#define I2S_WS_PIN 27 // Word select (LRCK)

#define I2S_DATA_PIN 26 // Data out (DIN)

// Audio parameters

#define SAMPLE_RATE 44100

#define BITS_PER_SAMPLE 16

#define CHANNELS 2 // Stereo

#define BUFFER_SIZE 1024 // Increased for smoother playback (adjust if needed)

const float masterGain = 0.5f; //01/20/26 Jo Pref: 0.5 = half previous max volume; tune to 0.4, 0.6, etc.

//reverb constants

#define DELAY_SAMPLES 22050 // ~0.5 s delay at 44.1 kHz

int16_t delayBuffer[DELAY_SAMPLES] = { 0 };

int delayPtr = 0;

const float reverbDecay = 0.4f; // 0.0–1.0 → how fast echo fades

const float reverbFeedback = 0.35f; // 0.0–0.6 → how much echo feeds back (keep low to avoid ringing)

// Solfeggio Frequencies (Hz)

const float frequencies[9] = { 174, 285, 396, 417, 528, 639, 741, 852, 963 };

int selectedFreqIndex = -1; // None selected initially

// UI Dimensions (240x320 portrait)

const int screenWidth = 240;

const int screenHeight = 320;

const int buttonSize = 70;

const int buttonSpacing = 10;

const int sliderHeight = 25; // Reduced to fit on screen

const int sliderSpacing = 10; // Adjusted spacing

const int sliderWidth = screenWidth - 20;

const int sliderX = 10;

const int leftVolSliderY = 255; // Adjusted to match touch top y~260, avoid button overlap

const int rightVolSliderY = leftVolSliderY + sliderHeight + sliderSpacing; // 255+25+10=290 to 315 (fits within 320)

// Audio controls

float leftVol = 0.5f; // Left ear volume (0-1)

float rightVol = 0.5f; // Right ear volume (0-1)

// Battery indicator (digital)

const int batteryPin = 35;

int batRectWidth = 30;

int batRectHeight = 5;

// Deep sleep (after inactivity)

unsigned long lastActivityTime = 0;

const unsigned long sleepTimeout = 20UL * 60 * 1000; // 20 minutes

// RTC data for surviving deep sleep

RTC_DATA_ATTR int bootCount = 0;

// Global buffer for samples

int16_t audioBuffer[BUFFER_SIZE * CHANNELS];

// NVS for state persistence

Preferences prefs;

// Touch state for hysteresis

bool isPressed = false;

//Function to generate audio data (sine wave with separate L/R volumes)

//01/20/26 mod to halve volume for Jo pref

void generateAudioBuffer(float frequency)

{

static float phase = 0.0;

const float phaseIncrement = 2.0 * M_PI * frequency / SAMPLE_RATE;

for (int i = 0; i < BUFFER_SIZE; i++)

{

int16_t sample = (int16_t)(sin(phase) * 32767.0); // full-scale dry sine

// Apply L/R volumes first (slider 0–1)

int32_t left = (int32_t)(sample * leftVol);

int32_t right = (int32_t)(sample * rightVol);

// Apply master gain (halves overall level without affecting slider mapping)

left = (int32_t)(left * masterGain);

right = (int32_t)(right * masterGain);

// Clip to 16-bit range

audioBuffer[2 * i] = (int16_t)constrain(left, INT16_MIN, INT16_MAX);

audioBuffer[2 * i + 1] = (int16_t)constrain(right, INT16_MIN, INT16_MAX);

phase += phaseIncrement;

if (phase >= 2.0 * M_PI) phase -= 2.0 * M_PI;

}

// Method to print the reason by which ESP32 has been awaken from sleep

void print_wakeup_reason()

{

esp_sleep_wakeup_cause_t wakeup_reason;

wakeup_reason = esp_sleep_get_wakeup_cause();

switch (wakeup_reason)

{

case ESP_SLEEP_WAKEUP_EXT0: Serial.println("Wakeup caused by external signal using RTC_IO"); break;

case ESP_SLEEP_WAKEUP_EXT1: Serial.println("Wakeup caused by external signal using RTC_CNTL"); break;

case ESP_SLEEP_WAKEUP_TIMER: Serial.println("Wakeup caused by timer"); break;

case ESP_SLEEP_WAKEUP_TOUCHPAD: Serial.println("Wakeup caused by touchpad"); break;

case ESP_SLEEP_WAKEUP_ULP: Serial.println("Wakeup caused by ULP program"); break;

default: Serial.printf("Wakeup was not caused by deep sleep: %d\n", wakeup_reason); break;

}

void setup()

{

Serial.begin(115200);

touchscreenSPI.begin(TOUCH_CLK, TOUCH_MISO, TOUCH_MOSI, TOUCH_CS);

ts.begin(touchscreenSPI);

ts.setRotation(1);

tft.init();

tft.invertDisplay(1);

tft.setRotation(2);

tft.fillScreen(TFT_BLACK);

tft.setTextColor(TFT_WHITE, TFT_BLACK);

tft.setTextSize(2);

pinMode(batteryPin, INPUT);

// Increment boot number and print it every reboot

++bootCount;

Serial.println("Boot number: " + String(bootCount));

// Print the wakeup reason for ESP32

print_wakeup_reason();

// Load state from NVS

prefs.begin("cyd_tone", false);

selectedFreqIndex = prefs.getInt("freqIndex", -1);

//leftVol = prefs.getFloat("leftVol", 0.5f);

//rightVol = prefs.getFloat("rightVol", 0.5f);

leftVol = prefs.getFloat("leftVol", 0.1f);//Jo pref

rightVol = prefs.getFloat("rightVol", 0.1f);//Jo pref

prefs.end();

// Configure I2S

i2s_config_t i2s_config = {

.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),

.sample_rate = SAMPLE_RATE,

.bits_per_sample = (i2s_bits_per_sample_t)BITS_PER_SAMPLE,

.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,

.communication_format = I2S_COMM_FORMAT_STAND_I2S,

.intr_alloc_flags = 0,

.dma_buf_count = 8,

.dma_buf_len = BUFFER_SIZE,

.use_apll = true, // Better clock accuracy for audio

.tx_desc_auto_clear = true,

.fixed_mclk = 0

};

i2s_pin_config_t pin_config = {

.bck_io_num = I2S_BCK_PIN,

.ws_io_num = I2S_WS_PIN,

.data_out_num = I2S_DATA_PIN,

.data_in_num = I2S_PIN_NO_CHANGE

};

// Install and start I2S driver

if (i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL) != ESP_OK)

{

Serial.println("I2S driver install failed");

while (1);

}

if (i2s_set_pin(I2S_NUM_0, &pin_config) != ESP_OK)

{

Serial.println("I2S pin set failed");

while (1);

}

Serial.println("I2S initialized.");

// Configure external wake up on GPIO36 (TOUCH_IRQ, wakes on low level when touched)

esp_sleep_enable_ext0_wakeup((gpio_num_t)TOUCH_IRQ, 0); // 0 for low level

Serial.println("[DEBUG] Wake setup done");

drawUI();

updateBatteryIndicator();

lastActivityTime = millis();

}

void loop()

{

// Prioritize audio

float currentFreq = (selectedFreqIndex >= 0) ? frequencies[selectedFreqIndex] : 0.0f;

generateAudioBuffer(currentFreq);

// Write buffer to I2S

size_t bytesWritten;

i2s_write(I2S_NUM_0, audioBuffer, sizeof(audioBuffer), &bytesWritten, portMAX_DELAY);

if (bytesWritten != sizeof(audioBuffer))

{

Serial.println("I2S write incomplete");

}

checkTouch(); // Check touch after audio to minimize interruption

updateBatteryIndicator();

if (millis() - lastActivityTime > sleepTimeout)

{

// Save state to NVS before sleep

prefs.begin("cyd_tone", false);

prefs.putInt("freqIndex", selectedFreqIndex);

prefs.putFloat("leftVol", leftVol);

prefs.putFloat("rightVol", rightVol);

prefs.end();

tft.fillScreen(TFT_BLACK); // Optional clear before sleep

Serial.println("Going to deep sleep now");

esp_deep_sleep_start(); // Deep sleep; resets on wake

}

// drawUI function

void drawUI()

{

tft.fillScreen(TFT_BLACK); // Clear screen

// Draw 3x3 buttons for frequencies

int row = 0;

int col = 0;

for (int i = 0; i < 9; i++)

{

int x = buttonSpacing + col * (buttonSize + buttonSpacing);

int y = buttonSpacing + row * (buttonSize + buttonSpacing);

tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE);

tft.setCursor(x + 10, y + 25);

tft.printf("%.0f", frequencies[i]);

col++;

if (col > 2)

{

col = 0;

row++;

}

// Left Volume slider (cyan) with label

tft.setTextSize(1);

tft.setCursor(sliderX - 30, leftVolSliderY + (sliderHeight / 2) - 4);

tft.print("L");

tft.setTextSize(2);

tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);

int leftFillWidth = static_cast<int>(leftVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, leftVolSliderY + 1, leftFillWidth, sliderHeight - 2, TFT_CYAN);

tft.fillRect(sliderX + 1 + leftFillWidth, leftVolSliderY + 1, sliderWidth - 2 - leftFillWidth, sliderHeight - 2, TFT_BLACK);

// Right Volume slider (green) with label

tft.setTextSize(1);

tft.setCursor(sliderX - 30, rightVolSliderY + (sliderHeight / 2) - 4);

tft.print("R");

tft.setTextSize(2);

tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE);

int rightFillWidth = static_cast<int>(rightVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, rightVolSliderY + 1, rightFillWidth, sliderHeight - 2, TFT_GREEN);

tft.fillRect(sliderX + 1 + rightFillWidth, rightVolSliderY + 1, sliderWidth - 2 - rightFillWidth, sliderHeight - 2, TFT_BLACK);

tft.drawRect(0, 0, batRectWidth, batRectHeight, TFT_WHITE);

redrawButtons(); // Apply selected state

}

// updateBatteryIndicator function using digitalRead

void updateBatteryIndicator()

{

int batLevel = digitalRead(batteryPin); // 1 = HIGH (good), 0 = LOW (low)

//Serial.println("Battery digital: " + String(batLevel)); // Debug

tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_BLACK); // Clear previous

if (batLevel == 0)

{

tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_RED); // Low

}

else

{

tft.fillRect(0, 0, batRectWidth, batRectHeight, TFT_GREEN); // Good

}

tft.drawRect(0, 0, batRectWidth, batRectHeight, TFT_WHITE);

}

// checkTouch function with hysteresis and state management

void checkTouch()

{

TS_Point p = ts.getPoint();

if (!isPressed)

{

if (p.z > TOUCH_PRESS_HIGH)

{

isPressed = true;

processTouch(p, true); // Initial press = true

}

else

{

if (p.z < TOUCH_PRESS_LOW)

{

isPressed = false;

// Process touch end if needed (not for this app)

}

else

{

processTouch(p, false); // Drag, initial press = false

}

delay(20); // Reduced debounce for responsiveness

}

// Helper to process touch (called on press start or drag)

void processTouch(TS_Point p, bool initialPress)

{

int x = map(p.y, TS_MINY, TS_MAXY, 0, screenWidth); // Inverted X

int y = map(p.x, TS_MINX, TS_MAXX, 0, screenHeight); // Inverted Y

//Serial.printf("Mapped X: %d, Y: %d, Z: %d\n", x, y, p.z);

// Check buttons first (only on initial press)

if (initialPress)

{

int row = (y - buttonSpacing) / (buttonSize + buttonSpacing);

int col = (x - buttonSpacing) / (buttonSize + buttonSpacing);

if (row >= 0 && row < 3 && col >= 0 && col < 3)

{

int index = row * 3 + col;

//Serial.printf("Button pressed: %d\n", index); // Debug

if (index != selectedFreqIndex)

{

selectedFreqIndex = index;

}

else

{

selectedFreqIndex = -1; // Toggle off

}

redrawButtons();

return; // Exit to avoid checking sliders

}

// Check left volume slider (allow drag, initial or ongoing)

if (y >= leftVolSliderY - 5 && y <= leftVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)

{

leftVol = static_cast<float>(x - sliderX) / sliderWidth;

leftVol = constrain(leftVol, 0.0f, 1.0f);

//Serial.printf("Left Vol adjusted to: %.2f\n", leftVol); // Debug

int fillWidth = static_cast<int>(leftVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, leftVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_CYAN);

tft.fillRect(sliderX + 1 + fillWidth, leftVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);

tft.drawRect(sliderX, leftVolSliderY, sliderWidth, sliderHeight, TFT_WHITE); // Redraw border

}

// Check right volume slider (allow drag, initial or ongoing)

if (y >= rightVolSliderY - 5 && y <= rightVolSliderY + sliderHeight + 5 && x >= sliderX && x <= sliderX + sliderWidth)

{

rightVol = static_cast<float>(x - sliderX) / sliderWidth;

rightVol = constrain(rightVol, 0.0f, 1.0f);

int fillWidth = static_cast<int>(rightVol * (sliderWidth - 2));

tft.fillRect(sliderX + 1, rightVolSliderY + 1, fillWidth, sliderHeight - 2, TFT_GREEN);

tft.fillRect(sliderX + 1 + fillWidth, rightVolSliderY + 1, sliderWidth - 2 - fillWidth, sliderHeight - 2, TFT_BLACK);

tft.drawRect(sliderX, rightVolSliderY, sliderWidth, sliderHeight, TFT_WHITE); // Redraw border

}

lastActivityTime = millis(); // Reset inactivity timer on touch

}

// redrawButtons function

void redrawButtons()

{

int row = 0;

int col = 0;

for (int i = 0; i < 9; i++)

{

int x = buttonSpacing + col * (buttonSize + buttonSpacing);

int y = buttonSpacing + row * (buttonSize + buttonSpacing);

tft.fillRect(x + 1, y + 1, buttonSize - 2, buttonSize - 2, (i == selectedFreqIndex) ? TFT_BLUE : TFT_BLACK);

tft.drawRect(x, y, buttonSize, buttonSize, TFT_WHITE); // Ensure border

tft.setCursor(x + 10, y + 25);

tft.printf("%.0f", frequencies[i]);

col++;

if (col > 2)

{

col = 0;

row++;

}

This all worked really well, except for one last major ‘gotcha’. We used GPIO35 (input only) to monitor the battery voltage, and implemented a small ‘slider’ style battery charge indicator at the very top of the display. A call to ‘analogRead()’ in loop() measured battery voltage, and this value was depicted by the length and color of the ‘slider’ display. Unfortunately it turned out that the ESP32 library required to run the PCM5102A DAC module conflicted with the Arduino library used for ‘analogRead()’ calls. As hard as this was to believe, it was real, and neither Grok nor I could figure any way around it. So, we wound up using ‘digitalRead()’ instead (which obviously will only return a ‘0’ or ‘1’) and displaying either a full-length green slider (battery good) or a full-length red slider (battery bad).

And the schematic:

MissChanger Klipper Configuration – Part 2

Leave a reply

Posted 31 October 2025

Last night I was able to verify that ‘klippy.log’ contains the entire in-lined configuration, after all includes and before parsing at the top of each entry. This should go a long way toward effective troubleshooting of the ‘No LEDs’ problem. However, when I started the process of implementing the Grok-suggested changes, I discovered that my printer will no longer connect to nhk0 or nhk1. I get the dreaded “mcu ‘nhk1’: Unable to connect” and/or “mcu ‘nhk0’: Unable to connect” error(s). Since I haven’t made any hardware changes this is kinda hard to believe, and at least so far I’m convinced this is yet another software-induced issue.

So, I plan to go back to my 251025 configuration (251025_wt_dock_printer.cfg & 251003_T0-Nitehawk-Revo-LDO.cfg). Yep! this configuration setup worked perfectly – no errors, and the restart was very rapid (maybe 2-5 seconds). LEDs on T0 also worked.

01 November 2025 Update:

After spending way too much time chasing the issue with LEDs not working on the T1(red) toolhead, I think I’m back going again. Again I’m starting with the following single-toolhead configuration:

251025_wt_dock_printer.cfg (printer.cfg with ‘with dock’ contents below the SAVE_CONFIG line)
251003_T0-Nitehawk-Revo-LDO.cfg

I modified printer.cfg in MainSail to change the max_y value for QGL from 225 to 275, and then ran SAVE_CONFIG_MODE to copy the changes to the ‘config_wt_dock.cfg’. This gives QGL a wider range of positions and should result in better gantry levelling in the ‘with-dock’ configuration.

I rechecked the x & y parking values for T0 and then ran TEST_DOCKING. The docking test went very well – very smooth, no hitches at all. An interesting note about TEST_DOCKING; if a recent HOME_ALL/QGL hasn’t been accomplished, it does another set before actually doing the docking moves.

Per recommendation from Grok I also added ‘[gcode_macro STATUS_MSG]’ to the top of printer.cfg to implement dual-destination logging – both to the status line and to the console log so I can see the full range of activity. The full macro is:

[respond]
default_type: echo

#11/01/25 added to direct output to both the status line and the console
[gcode_macro STATUS_MSG]
description: LCD + instant console log (NO TIME - WORKS 100%)
gcode:
    {% set msg  = params.MSG|default("") %}
    {% set type = params.TYPE|default("echo") %}
    {% set tool = params.TOOL|default(printer.tool_probe_endstop.active_tool_number|int) %}
    {% set tool = (tool|int if tool != -1 else 0)|string %}
    {% if msg == "" %}
      { action_raise_error("STATUS_MSG: Missing MSG=") }
    {% endif %}

    M117 T{tool}: {msg}
    RESPOND TYPE={type} MSG="T{tool}: {msg}"

[respond]

default_type: echo

#11/01/25 added to direct output to both the status line and the console

[gcode_macro STATUS_MSG]

description: LCD + instant console log (NO TIME - WORKS 100%)

gcode:

{% set msg = params.MSG|default("") %}

{% set type = params.TYPE|default("echo") %}

{% set tool = params.TOOL|default(printer.tool_probe_endstop.active_tool_number|int) %}

{% set tool = (tool|int if tool != -1 else 0)|string %}

{% if msg == "" %}

{ action_raise_error("STATUS_MSG: Missing MSG=") }

{% endif %}

M117 T{tool}: {msg}

RESPOND TYPE={type} MSG="T{tool}: {msg}"

and it gets called like this:

STATUS_MSG MSG="Heating T1" TYPE=echo     # Green console
STATUS_MSG MSG="ERROR: Timeout!" TYPE=error  # Red `!!

1 2	STATUS_MSG MSG="Heating T1" TYPE=echo # Green console STATUS_MSG MSG="ERROR: Timeout!" TYPE=error # Red `!!

Note that to make outputs from a STATUS_MSG command immediately visible in the console window in, the standard console filter “Hide Temperatures” must be disabled, and a new filter added to suppress the status messages that are emitted by the ‘STATUS’ line at the end of the STATUS_MSG macro (the STATUS line is required to force a console window update). Interestingly, the ‘Hide Temperatures’ filter is supposed to suppress the 1-2 second period temp reports, but disabling this filter still doesn’t result in a blizzard of temp reports. From Grok:

Why No Automatic Temps in Console?Klipper Design: Klipper doesn't "spam" real-time temperature updates to the console tab (which is primarily for G-code commands/responses and explicit messages like RESPOND/M118). Instead:Periodic stats (including temps) are logged to klippy.log (every ~1 second when active, like during heating).
Temps only appear in the console when explicitly queried (e.g., via M105) or during certain events (e.g., heating errors).

UI Role: The Console tab in Fluidd/Mainsail shows interactive G-code output, not the full backend log. The "Hide Temperatures" filter only affects M105-like responses (hiding them to reduce clutter), but since no automatic reports are sent, disabling it won't "unlock" them.

Why No Automatic Temps in Console?Klipper Design: Klipper doesn't "spam" real-time temperature updates to the console tab (which is primarily for G-code commands/responses and explicit messages like RESPOND/M118). Instead:Periodic stats (including temps) are logged to klippy.log (every ~1 second when active, like during heating).

Temps only appear in the console when explicitly queried (e.g., via M105) or during certain events (e.g., heating errors).

UI Role: The Console tab in Fluidd/Mainsail shows interactive G-code output, not the full backend log. The "Hide Temperatures" filter only affects M105-like responses (hiding them to reduce clutter), but since no automatic reports are sent, disabling it won't "unlock" them.

So I’m happy with the current setup; ‘Hide Temperatures’ filter OFF, ‘Hide Klipper State’ OFF, ‘STATUS’ line removed from STATUS_MSG macro.

02 November 2025 Update:

I saved the three files relevant to the STATUS_MGS macro adventure to a folder named

‘20251102-141756 STATUS_MSG Macro Files’ in the MissChanger/Config Files folder, so now I can start moving forward again toward dual-toolhead nirvana. I plan to do this as follows:

Add [include 251029_T1-Nitehawk-Revo-LDO.cfg] to printer.cfg just before the SAVE_CONFIG section. this file in turn includes 251002_T1_stealthburner_leds.cfg

OK, this came up OK, And now both ‘T0’ and ‘T1’ are available in the ‘Extruder’ tab in the dashboard – yay! From Klippy.log I can see that the T0/T1 include files have been properly in-lined into printer.cfg, along with the T0/T1 neopixel LED files, with NO errors. It’s really nice being able to see the entire config chain in one file.

Looking at klippy.log, I see the following ‘parking’ positions for T0 & T1:

T0: params_park_x = 69.00, params_park_y = 90.00, params_park_z = 250.0
T1: params_park_x = 245.0, params_park_y = 91.50, params_park_z = 250.0

These values should be pretty close to correct for tool changes. I have already verified that T0 can be docked and undocked using TEST_DOCKING, but I still need to re-verify the values for T1.

03 November 2025 Update:

At the moment, neither toolhead has been selected, so I’m going to manually swap T0 out (docked), and T1 in (place it on the carrier), and then command a HOME_ALL (G28). This *should* cause T1 to be detected as the active toolhead. Yes!! T1 was detected as the active toolhead. Wow! I actually knew something!

OK, now I’m going to re-verify the parking positions using manual moves, and then I’m going to try TEST_DOCKING. WooHoo! Test Docking on T1 worked, and now I can even change toolheads under program control, as shown in the following short videos:

Docking test with the TEST_DOCKING macro

Demonstrating the ability to change back and forth between T0 & T1

Now that I have tool changing working, I think we are about done with the hardware configuration aspects of the MissChanger mod. I think the next step is to get Prusa Slicer configured to handle dual extruders, and get some test prints going – woo hoo!

Went back through MissChanger’s Prusa Slicer settings, so I think I have this correct. Tried a T1 test print with ‘251103_T1_Strap tensioner optimized_v003.gcode’, but it failed with the error ‘Error evaluating ‘gcode_macro PRINT_START:gcode’: jinja2.exceptions.UndefinedError: ‘dict object’ has no attribute ‘BED‘

I’m thinking that this error is yet another instance of my old single-toolhead PRINT_START macro not being able to handle multiple toolheads.

Grok agreed, and proposed the following PRINT_START macro upgrade:

[gcode_macro PRINT_START]
description: Multi-tool start (heat active extruder + bed/chamber)
gcode:
    {% set TOOL = printer.toolchanger.tool_number|int %}  # Active tool
    {% set EXTRUDER = 'extruder' + (TOOL if TOOL > 0 else '') %}  # 'extruder' (T0) or 'extruder1' (T1)
    {% set BED = params.BED|default(60)|int %}  # Default 60 if missing
    {% set EXTRUDER_TEMP = params.EXTRUDER|default(200)|int %}  # Default 200
    {% set CHAMBER = params.CHAMBER|default(0)|int %}  # Default 0 (off)

    STATUS_MSG MSG="PRINT_START: T{TOOL} Bed={BED} Extruder={EXTRUDER_TEMP} Chamber={CHAMBER}" TYPE=echo

    G28  # Home
    BED_MESH_CLEAR
    QUAD_GANTRY_LEVEL  # Level

    # Heat Bed
    SET_HEATER_TEMPERATURE HEATER=heater_bed TARGET={BED}
    TEMPERATURE_WAIT SENSOR=heater_bed MINIMUM={BED-2} MAXIMUM={BED+2}

    # Heat Active Extruder
    SET_HEATER_TEMPERATURE HEATER={EXTRUDER} TARGET={EXTRUDER_TEMP}
    TEMPERATURE_WAIT SENSOR={EXTRUDER} MINIMUM={EXTRUDER_TEMP-2} MAXIMUM={EXTRUDER_TEMP+2}

    # Chamber (if >0)
    {% if CHAMBER > 0 %}
        STATUS_MSG MSG="Heating Chamber to {CHAMBER}"
        # Use chamber heater/fan logic (add your chamber macro if needed)
        M141 S{CHAMBER}  # Or your chamber heat command
        TEMPERATURE_WAIT SENSOR=chamber MINIMUM={CHAMBER-2} MAXIMUM={CHAMBER+2}
    {% endif %}

    BED_MESH_CALIBRATE  # Mesh
    G90  # Absolute

    STATUS_READY  # LEDs ready!

[gcode_macro PRINT_START]

description: Multi-tool start (heat active extruder + bed/chamber)

gcode:

{% set TOOL = printer.toolchanger.tool_number|int %} # Active tool

{% set EXTRUDER = 'extruder' + (TOOL if TOOL > 0 else '') %} # 'extruder' (T0) or 'extruder1' (T1)

{% set BED = params.BED|default(60)|int %} # Default 60 if missing

{% set EXTRUDER_TEMP = params.EXTRUDER|default(200)|int %} # Default 200

{% set CHAMBER = params.CHAMBER|default(0)|int %} # Default 0 (off)

STATUS_MSG MSG="PRINT_START: T{TOOL} Bed={BED} Extruder={EXTRUDER_TEMP} Chamber={CHAMBER}" TYPE=echo

G28 # Home

BED_MESH_CLEAR

QUAD_GANTRY_LEVEL # Level

# Heat Bed

SET_HEATER_TEMPERATURE HEATER=heater_bed TARGET={BED}

TEMPERATURE_WAIT SENSOR=heater_bed MINIMUM={BED-2} MAXIMUM={BED+2}

# Heat Active Extruder

SET_HEATER_TEMPERATURE HEATER={EXTRUDER} TARGET={EXTRUDER_TEMP}

TEMPERATURE_WAIT SENSOR={EXTRUDER} MINIMUM={EXTRUDER_TEMP-2} MAXIMUM={EXTRUDER_TEMP+2}

# Chamber (if >0)

{% if CHAMBER > 0 %}

STATUS_MSG MSG="Heating Chamber to {CHAMBER}"

# Use chamber heater/fan logic (add your chamber macro if needed)

M141 S{CHAMBER} # Or your chamber heat command

TEMPERATURE_WAIT SENSOR=chamber MINIMUM={CHAMBER-2} MAXIMUM={CHAMBER+2}

{% endif %}

BED_MESH_CALIBRATE # Mesh

G90 # Absolute

STATUS_READY # LEDs ready!

However, before implementing this change, I saved off the current config file set (printer.cfg, 251003_T0_Nitehawk-Revo-LDO.cfg, 250922_TO_stealthburner_leds.cfg, 251029_T1_Nitehawk-Revo-LDO.cfg, 251002_T1_stealthburner_leds.cfg) to config-20251103-113415.zip and put it in C:\Users\Frank\Documents\3D Projects\Voron Printer\MissChanger\Dock.

Then I replaced my PRINT_START with Grok’s version, saved, and restarted. I also reset (rolled-over) Klippy.log, which caused another save/restart. Then I tried the print again.

Side note: After starting the print, the console window started getting ‘spammed’ by temperature messages – the behavior that resulted in having a ‘hide temperatures’ filter. I activated the filter and the spam stopped (not sure if that affected my STATUS_MSG behavior or not).

Hmm, activating the ‘hide temperatures’ filter didn’t stop the temps span – uh oh!

Well, that went about as well as could be expected, meaning ‘not very well at all’ :(.

Grok used our STATUS_MSG macro:

STATUS_MSG MSG="PRINT_START: T{TOOL} Bed={BED} Extruder={EXTRUDER_TEMP} Chamber={CHAMBER}" TYPE=echo

1	STATUS_MSG MSG="PRINT_START: T{TOOL} Bed={BED} Extruder={EXTRUDER_TEMP} Chamber={CHAMBER}" TYPE=echo

but the result was “T1: PRINT_START: T{TOOL} Bed={BED} Extruder={EXTRUDER_TEMP} Chamber={CHAMBER}”. In other words, none of the {} parameters were expanded properly. And to add insult to injury, even that much didn’t show up until I hit F5 (or disabled the ‘hide temperatures’ filter).

The extruder temperature went up to 200C and then stayed there, until eventually resulting in a “Extrude below min temperature”. I’m actually not even sure the T1 extruder ever got hot AT ALL – have to check.

Also got another error – “The value ‘sb_leds’ is not valid for LED”. This sounds like an issue with naming conventions in printer.cfg

The reason the ‘Extruder below temps’ error occurred was because the T0 extruder was being heated, but the desired T1 extruder was not. I confirmed I could manually set T1 to a temp at the printer KlipperScreen and T1’s extruder actually got hot. So, the number mixup may well be in PRINT_START or in the Gcode

07 November 2025 Update:

These last few days I have been working on getting the various stealthburner LED color/intensity combinations working on both T0 & T1 toolheads, and wound up with a totally different configuration than the ‘stock’ MissChanger. Instead of two complete sets of ‘STATUS_xxx’ LED-setting macros, Grok talked me through creating a single set of macro files called ‘251103_shared_stealthburner_leds.cfg’ containing all the macro code, and separate very small ‘251103_T0/T1_stealthburner_leds.cfg. All the T0/T1 files to is define the mcu pin number to be used. When a ‘STATUS_XXX’ macro is called, it first figures out which toolhead is active, and then passes a new ‘LED’ parameter to the appropriate led-setting macro (typically ‘_set_sb_leds_by_name’). This took a long while to work through all the hardware and software problems, but eventually (with a LOT of help from Grok), I got it figured out. All the status codes work now, with both T0 & T1. The related config files were all saved to the ‘C:\Users\Frank\Documents\3D Projects\Voron Printer\MissChanger\Config Files\251107_stealthburner_LED_config’ folder.

The saga continues in

GPS-enabled Dash Cam Power Switch

Leave a reply

Posted 10 November 2025

A while ago I got a dash cam for my truck, thinking that would be a good thing to have if someone tries to cash in on my insurance by deliberately backing into me on the road. So, I got this dash cam, mounted it on the windshield of my truck, and life was good. Then my wife and I went out of town for a few days and when we came back (at 2am), the truck battery was dead and it took us a while (and a new car battery) to recover. This happened one more time, fortunately in my garage, and I decided I was going to have to come up with a way of killing the power to the dash cam when (or shortly thereafter) the truck stopped moving.

I researched the power outlets in my truck (a 2011 Ford F-150) and found that the power to the SYNC system was (at least theoretically) on only when the engine was running. Unfortunately, I had pulled the fuse for this circuit years before to keep SYNC from grabbing my phone’s Bluetooth signal even though I had long ago UNpartnered SYNC and my phone, so that was out.

My next idea was to construct a power switch around one of the recent GPS modules available cheaply, such as this one. I looked around a little bit, and found the nice ‘TinyGPS++’ library, and used a Sparkfun Pro Micro (the Arduino version) from my parts box to interface to the GPS module. Another search and I found an example sketch, and I was in business. After the usual number of errors I got the sketch working, with data displayed in the serial window of my Windows 11/Visual Studio/Visual Micro setup.

Unfortunately, all the data came back as ‘INVALID’, mostly because we live in an earth-sheltered house, and that pretty much snuffs out any GPS signals. The next day I took my laptop and the breadboard for a spin, and the GPS module started spitting out valid data about 30 seconds after I backed out of the garage. The data looks like this:

Location: INVALID Date/Time: 11/10/2025 20:12:49.00
Location: INVALID Date/Time: 11/10/2025 20:12:49.00
Location: INVALID Date/Time: 11/10/2025 20:12:49.00
Location: INVALID Date/Time: 11/10/2025 20:12:49.00
Location: INVALID Date/Time: 11/10/2025 20:12:49.00
Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98
Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98
Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98
Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98
Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98
Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98
Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00
Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00
Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00
Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00
Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00
Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00
Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00
Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00
Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00
Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00
Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00
Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00

Location: INVALID Date/Time: 11/10/2025 20:12:49.00

Location: 39.887649,-83.153770 Date/Time: 11/10/2025 20:12:49.98

Location: 39.887664,-83.153671 Date/Time: 11/10/2025 20:12:51.00

Location: 39.887680,-83.153594 Date/Time: 11/10/2025 20:12:52.00

This isn’t particularly useful for my project, as I had planned to use speed < MPH_THRESHOLD for some time > ELAPSED_TIME_THRESHOLD, maybe something like 10-20 minutes. The above data has the time I need, but not the speed. Maybe the TinyGPS++ library has that capability?

Yes, the TinyGPS++ library does have the methods I want. Here’s the code I came up with:

/*
    Name:       GT_U7_GPS.ino
    Created:	11/8/2025 9:51:34 PM
    Author:     FRANK_XPS_9530\Frank
*/

#define MIN_SPEED_THRESHOLD_MPH 5
#define MAX_TOO_SLOW_TIME_THRESHOLD_MIN 5 //for debug - change to 20 later
#define POWER_SWITCH_PIN 10

//#include <TinyGPS++.h>
#include <TinyGPSPlus.h>
#include <SoftwareSerial.h>
#include <ElapsedMillis.h>
elapsedSeconds SecSinceTooSlow;

// Define the pins for RX and TX
static const int RXPin = 8, TXPin = 9;
static const uint32_t GPSBaud = 9600;
// Create a TinyGPS++ object
TinyGPSPlus gps;
// Create a SoftwareSerial object
SoftwareSerial ss(RXPin, TXPin);
void setup() 
{
  Serial.begin(115200);
  ss.begin(GPSBaud);
  Serial.println(F("GPS Start"));
  SecSinceTooSlow = 0;
}

void loop() {
  // Read data from the GPS module
  while (ss.available() > 0) 
  {
    if (gps.encode(ss.read())) 
    {
      displayInfo();

      //update power switch state
      digitalWrite(POWER_SWITCH_PIN, UpdatePowerCondition());

    }
  }
  // Check if no GPS data is received
  if (millis() > 5000 && gps.charsProcessed() < 10) 
  {
    Serial.println(F("No GPS detected: check wiring."));
    while (true);
  }
}
void displayInfo() 
{
  //Serial.print(F("Location: "));
  //if (gps.location.isValid()) {
  //  Serial.print(gps.location.lat(), 6);
  //  Serial.print(F(","));
  //  Serial.print(gps.location.lng(), 6);
  //}
  //else 
  //{
  //  Serial.print(F("INVALID"));
  //}

  //11/10/25 gfp dropped 'Location', added 'Speed'
  Serial.print(F("Speed: "));
  if(gps.speed.isValid())
  {
    Serial.print(gps.speed.mph(), 3);
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.print(F(" Date/Time: "));
  if (gps.date.isValid()) 
  {
    Serial.print(gps.date.month());
    Serial.print(F("/"));
    Serial.print(gps.date.day());
    Serial.print(F("/"));
    Serial.print(gps.date.year());
  }
  else 
  {
    Serial.print(F("INVALID"));
  }
  Serial.print(F(" "));
  //if (gps.time.isValid()) 
  //{
  //  if (gps.time.hour() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.hour());
  //  Serial.print(F(":"));
  //  if (gps.time.minute() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.minute());
  //  Serial.print(F(":"));
  //  if (gps.time.second() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.second());
  //  Serial.print(F("."));
  //  if (gps.time.centisecond() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.centisecond());
  //}
  //else {
  //  Serial.print(F("INVALID"));
  //}
  PrintTime();
  Serial.println();
}

bool UpdatePowerCondition()
{
  //Purpose: Determine if output power should be enabled or disabled
  //Inputs: SecSinceTooSlow = UnsignedLong indicating sec since speed fell below threshold
  //Outputs: False if speed < UpdatePowerCondition() AND elapsed time under speed threshold is > MAX_TIME_THRESHOLD_MIN
  //         Otherwise True.
  //Procedure:
  //  Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.
  //  Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE

  bool result = false; //default is to kill the power
//Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.
  if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH)
  {
    Serial.print("Power ON at: ");
    PrintTime();
    Serial.println();

    SecSinceTooSlow = 0; //reset elapsed time
    result = true;
  }

//Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE
  if (gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH && SecSinceTooSlow >= (unsigned long)(60*MAX_TOO_SLOW_TIME_THRESHOLD_MIN))
  {
    Serial.print("Power OFF at: ");
    PrintTime();
    Serial.println();
    result = true;
  }
  
  return result;
}
void PrintTime()
{
  if (gps.time.isValid())
  {
    if (gps.time.hour() < 10) Serial.print(F("0"));
    Serial.print(gps.time.hour());
    Serial.print(F(":"));
    if (gps.time.minute() < 10) Serial.print(F("0"));
    Serial.print(gps.time.minute());
    Serial.print(F(":"));
    if (gps.time.second() < 10) Serial.print(F("0"));
    Serial.print(gps.time.second());
    Serial.print(F("."));
    if (gps.time.centisecond() < 10) Serial.print(F("0"));
    Serial.print(gps.time.centisecond());
  }
  else 
  {
    Serial.print(F("INVALID"));
  }
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

Name: GT_U7_GPS.ino

Created: 11/8/2025 9:51:34 PM

Author: FRANK_XPS_9530\Frank

#define MIN_SPEED_THRESHOLD_MPH 5

#define MAX_TOO_SLOW_TIME_THRESHOLD_MIN 5 //for debug - change to 20 later

#define POWER_SWITCH_PIN 10

//#include <TinyGPS++.h>

#include <TinyGPSPlus.h>

#include <SoftwareSerial.h>

#include <ElapsedMillis.h>

elapsedSeconds SecSinceTooSlow;

// Define the pins for RX and TX

static const int RXPin = 8, TXPin = 9;

static const uint32_t GPSBaud = 9600;

// Create a TinyGPS++ object

TinyGPSPlus gps;

// Create a SoftwareSerial object

SoftwareSerial ss(RXPin, TXPin);

void setup()

{

Serial.begin(115200);

ss.begin(GPSBaud);

Serial.println(F("GPS Start"));

SecSinceTooSlow = 0;

}

void loop() {

// Read data from the GPS module

while (ss.available() > 0)

{

if (gps.encode(ss.read()))

{

displayInfo();

//update power switch state

digitalWrite(POWER_SWITCH_PIN, UpdatePowerCondition());

}

// Check if no GPS data is received

if (millis() > 5000 && gps.charsProcessed() < 10)

{

Serial.println(F("No GPS detected: check wiring."));

while (true);

}

void displayInfo()

{

//Serial.print(F("Location: "));

//if (gps.location.isValid()) {

// Serial.print(gps.location.lat(), 6);

// Serial.print(F(","));

// Serial.print(gps.location.lng(), 6);

//}

//else

//{

// Serial.print(F("INVALID"));

//}

//11/10/25 gfp dropped 'Location', added 'Speed'

Serial.print(F("Speed: "));

if(gps.speed.isValid())

{

Serial.print(gps.speed.mph(), 3);

}

else

{

Serial.print(F("INVALID"));

}

Serial.print(F(" Date/Time: "));

if (gps.date.isValid())

{

Serial.print(gps.date.month());

Serial.print(F("/"));

Serial.print(gps.date.day());

Serial.print(F("/"));

Serial.print(gps.date.year());

}

else

{

Serial.print(F("INVALID"));

}

Serial.print(F(" "));

//if (gps.time.isValid())

//{

// if (gps.time.hour() < 10) Serial.print(F("0"));

// Serial.print(gps.time.hour());

// Serial.print(F(":"));

// if (gps.time.minute() < 10) Serial.print(F("0"));

// Serial.print(gps.time.minute());

// Serial.print(F(":"));

// if (gps.time.second() < 10) Serial.print(F("0"));

// Serial.print(gps.time.second());

// Serial.print(F("."));

// if (gps.time.centisecond() < 10) Serial.print(F("0"));

// Serial.print(gps.time.centisecond());

//}

//else {

// Serial.print(F("INVALID"));

//}

PrintTime();

Serial.println();

}

bool UpdatePowerCondition()

{

//Purpose: Determine if output power should be enabled or disabled

//Inputs: SecSinceTooSlow = UnsignedLong indicating sec since speed fell below threshold

//Outputs: False if speed < UpdatePowerCondition() AND elapsed time under speed threshold is > MAX_TIME_THRESHOLD_MIN

// Otherwise True.

//Procedure:

// Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.

// Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE

bool result = false; //default is to kill the power

//Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.

if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH)

{

Serial.print("Power ON at: ");

PrintTime();

Serial.println();

SecSinceTooSlow = 0; //reset elapsed time

result = true;

}

//Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE

if (gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH && SecSinceTooSlow >= (unsigned long)(60*MAX_TOO_SLOW_TIME_THRESHOLD_MIN))

{

Serial.print("Power OFF at: ");

PrintTime();

Serial.println();

result = true;

}

return result;

}

void PrintTime()

{

if (gps.time.isValid())

{

if (gps.time.hour() < 10) Serial.print(F("0"));

Serial.print(gps.time.hour());

Serial.print(F(":"));

if (gps.time.minute() < 10) Serial.print(F("0"));

Serial.print(gps.time.minute());

Serial.print(F(":"));

if (gps.time.second() < 10) Serial.print(F("0"));

Serial.print(gps.time.second());

Serial.print(F("."));

if (gps.time.centisecond() < 10) Serial.print(F("0"));

Serial.print(gps.time.centisecond());

}

else

{

Serial.print(F("INVALID"));

}

Now to test the code. I found a free web-based app called NMEASimulator by panazzolo.com that allows me to set a course with heading and speed, and then output the corresponding NMEA sentences to a COM port. With a USB-to-Serial adapter I should be able to replace the data lines from the GPS module with the simulator output and test the code.

12 November 2025 Update:

My USB-to-Serial adapter arrived today, and I was able to connect it to my GPS switching board, bypassing the actual GPS. Then I was able to use the NMEA simulator to transmit NMEA messages to the Sparkfun Pro Micro and test the Dash Cam power switch algorithm.

This combination worked great! I started the action, NMEA sentences started flowing to the Pro Micro which output decoded speed values to the debug window. I could run the speed up and down and watch the program toggle power to the load (39-Ohm power resistor) in response to the speed changes. The only things left to do are to build an enclosure and change the time and speed threshold parameters. Here’s the ‘final’ code:

/*
    Name:       GT_U7_GPS.ino
    Created:	11/8/2025 9:51:34 PM
    Author:     FRANK_XPS_9530\Frank
*/

#define MIN_SPEED_THRESHOLD_MPH 5
#define MAX_TOO_SLOW_TIME_THRESHOLD_MIN 1 //for debug - change to 20 later
#define POWER_SWITCH_PIN 10

//#include <TinyGPS++.h>
#include <TinyGPSPlus.h>
#include <SoftwareSerial.h>
#include <ElapsedMillis.h>
elapsedSeconds SecSinceTooSlow;

// Define the pins for RX and TX
static const int RXPin = 8, TXPin = 9;
static const uint32_t GPSBaud = 9600;

// Create a TinyGPS++ object
TinyGPSPlus gps;

// Create a SoftwareSerial object
SoftwareSerial ss(RXPin, TXPin);

void setup() 
{
  Serial.begin(115200);
  ss.begin(GPSBaud);
  Serial.println(F("GPS Start"));
  SecSinceTooSlow = 0;
  pinMode(POWER_SWITCH_PIN, OUTPUT);
}

void loop() {
  // Read data from the GPS module
  while (ss.available() > 0) 
  {
    if (gps.encode(ss.read())) 
    {
      displayInfo();

      //update power switch state
      if (!UpdatePowerCondition())
      {
        Serial.println("UpdatePowerCondition() returned FALSE - Powering OFF");
        digitalWrite(POWER_SWITCH_PIN, LOW);

      }
      else
      {
        Serial.println("UpdatePowerCondition() returned TRUE - Powering ON");
        digitalWrite(POWER_SWITCH_PIN, HIGH);
      }

    }
  }
  // Check if no GPS data is received
  if (millis() > 5000 && gps.charsProcessed() < 10) 
  {
    Serial.println(F("No GPS detected: check wiring."));
    while (true);
  }
}
void displayInfo() 
{
  //Serial.print(F("Location: "));
  //if (gps.location.isValid()) {
  //  Serial.print(gps.location.lat(), 6);
  //  Serial.print(F(","));
  //  Serial.print(gps.location.lng(), 6);
  //}
  //else 
  //{
  //  Serial.print(F("INVALID"));
  //}

  //11/10/25 gfp dropped 'Location', added 'Speed'
  Serial.print(F("Speed: "));
  if(gps.speed.isValid())
  {
    Serial.print(gps.speed.mph(), 3);
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.print(F(" Date/Time: "));
  if (gps.date.isValid()) 
  {
    Serial.print(gps.date.month());
    Serial.print(F("/"));
    Serial.print(gps.date.day());
    Serial.print(F("/"));
    Serial.print(gps.date.year());
  }
  else 
  {
    Serial.print(F("INVALID"));
  }
  Serial.print(F(" "));
  //if (gps.time.isValid()) 
  //{
  //  if (gps.time.hour() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.hour());
  //  Serial.print(F(":"));
  //  if (gps.time.minute() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.minute());
  //  Serial.print(F(":"));
  //  if (gps.time.second() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.second());
  //  Serial.print(F("."));
  //  if (gps.time.centisecond() < 10) Serial.print(F("0"));
  //  Serial.print(gps.time.centisecond());
  //}
  //else {
  //  Serial.print(F("INVALID"));
  //}
  PrintTime();
  Serial.println();
}

bool UpdatePowerCondition()
{
  //Purpose: Determine if output power should be enabled or disabled
  //Inputs: SecSinceTooSlow = UnsignedLong indicating sec since speed fell below threshold
  //Outputs: False if speed < UpdatePowerCondition() AND elapsed time under speed threshold is > MAX_TIME_THRESHOLD_MIN
  //         Otherwise True.
  //Procedure:
  //  Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.
  //  Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE

  bool result = false; //default is to kill the power

//Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.
  if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH)
  {
    Serial.print("Power ON at: ");
    PrintTime();
    Serial.println();

    Serial.print("Speed greater than threshold; Resetting SecSinceTooSlow from "); Serial.println(SecSinceTooSlow);
    SecSinceTooSlow = 0; //reset elapsed time
    
    result = true;
  }

//Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE
  if (gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH)
  {
    Serial.print("Speed below threshold at "); PrintTime(); Serial.print(" with SecSinceTooSlow = "); Serial.println(SecSinceTooSlow);

    if (SecSinceTooSlow >= (unsigned long)(60 * MAX_TOO_SLOW_TIME_THRESHOLD_MIN))
    {
      result = false;
    }
    else
    {
      result = true;
    }
  }
  
  return result;
}

void PrintTime()
{
  if (gps.time.isValid())
  {
    if (gps.time.hour() < 10) Serial.print(F("0"));
    Serial.print(gps.time.hour());
    Serial.print(F(":"));
    if (gps.time.minute() < 10) Serial.print(F("0"));
    Serial.print(gps.time.minute());
    Serial.print(F(":"));
    if (gps.time.second() < 10) Serial.print(F("0"));
    Serial.print(gps.time.second());
    Serial.print(F("."));
    if (gps.time.centisecond() < 10) Serial.print(F("0"));
    Serial.print(gps.time.centisecond());
  }
  else 
  {
    Serial.print(F("INVALID"));
  }
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

Name: GT_U7_GPS.ino

Created: 11/8/2025 9:51:34 PM

Author: FRANK_XPS_9530\Frank

#define MIN_SPEED_THRESHOLD_MPH 5

#define MAX_TOO_SLOW_TIME_THRESHOLD_MIN 1 //for debug - change to 20 later

#define POWER_SWITCH_PIN 10

//#include <TinyGPS++.h>

#include <TinyGPSPlus.h>

#include <SoftwareSerial.h>

#include <ElapsedMillis.h>

elapsedSeconds SecSinceTooSlow;

// Define the pins for RX and TX

static const int RXPin = 8, TXPin = 9;

static const uint32_t GPSBaud = 9600;

// Create a TinyGPS++ object

TinyGPSPlus gps;

// Create a SoftwareSerial object

SoftwareSerial ss(RXPin, TXPin);

void setup()

{

Serial.begin(115200);

ss.begin(GPSBaud);

Serial.println(F("GPS Start"));

SecSinceTooSlow = 0;

pinMode(POWER_SWITCH_PIN, OUTPUT);

}

void loop() {

// Read data from the GPS module

while (ss.available() > 0)

{

if (gps.encode(ss.read()))

{

displayInfo();

//update power switch state

if (!UpdatePowerCondition())

{

Serial.println("UpdatePowerCondition() returned FALSE - Powering OFF");

digitalWrite(POWER_SWITCH_PIN, LOW);

}

else

{

Serial.println("UpdatePowerCondition() returned TRUE - Powering ON");

digitalWrite(POWER_SWITCH_PIN, HIGH);

}

// Check if no GPS data is received

if (millis() > 5000 && gps.charsProcessed() < 10)

{

Serial.println(F("No GPS detected: check wiring."));

while (true);

}

void displayInfo()

{

//Serial.print(F("Location: "));

//if (gps.location.isValid()) {

// Serial.print(gps.location.lat(), 6);

// Serial.print(F(","));

// Serial.print(gps.location.lng(), 6);

//}

//else

//{

// Serial.print(F("INVALID"));

//}

//11/10/25 gfp dropped 'Location', added 'Speed'

Serial.print(F("Speed: "));

if(gps.speed.isValid())

{

Serial.print(gps.speed.mph(), 3);

}

else

{

Serial.print(F("INVALID"));

}

Serial.print(F(" Date/Time: "));

if (gps.date.isValid())

{

Serial.print(gps.date.month());

Serial.print(F("/"));

Serial.print(gps.date.day());

Serial.print(F("/"));

Serial.print(gps.date.year());

}

else

{

Serial.print(F("INVALID"));

}

Serial.print(F(" "));

//if (gps.time.isValid())

//{

// if (gps.time.hour() < 10) Serial.print(F("0"));

// Serial.print(gps.time.hour());

// Serial.print(F(":"));

// if (gps.time.minute() < 10) Serial.print(F("0"));

// Serial.print(gps.time.minute());

// Serial.print(F(":"));

// if (gps.time.second() < 10) Serial.print(F("0"));

// Serial.print(gps.time.second());

// Serial.print(F("."));

// if (gps.time.centisecond() < 10) Serial.print(F("0"));

// Serial.print(gps.time.centisecond());

//}

//else {

// Serial.print(F("INVALID"));

//}

PrintTime();

Serial.println();

}

bool UpdatePowerCondition()

{

//Purpose: Determine if output power should be enabled or disabled

//Inputs: SecSinceTooSlow = UnsignedLong indicating sec since speed fell below threshold

//Outputs: False if speed < UpdatePowerCondition() AND elapsed time under speed threshold is > MAX_TIME_THRESHOLD_MIN

// Otherwise True.

//Procedure:

// Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.

// Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE

bool result = false; //default is to kill the power

//Step1: Get speed from GPS. If > threshold, reset SecSinceTooSlow to zero and return TRUE.

if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH)

{

Serial.print("Power ON at: ");

PrintTime();

Serial.println();

Serial.print("Speed greater than threshold; Resetting SecSinceTooSlow from "); Serial.println(SecSinceTooSlow);

SecSinceTooSlow = 0; //reset elapsed time

result = true;

}

//Step2: If speed < threshold AND SecSinceTooSlow > threshold, return FALSE

if (gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH)

{

Serial.print("Speed below threshold at "); PrintTime(); Serial.print(" with SecSinceTooSlow = "); Serial.println(SecSinceTooSlow);

if (SecSinceTooSlow >= (unsigned long)(60 * MAX_TOO_SLOW_TIME_THRESHOLD_MIN))

{

result = false;

}

else

{

result = true;

}

return result;

}

void PrintTime()

{

if (gps.time.isValid())

{

if (gps.time.hour() < 10) Serial.print(F("0"));

Serial.print(gps.time.hour());

Serial.print(F(":"));

if (gps.time.minute() < 10) Serial.print(F("0"));

Serial.print(gps.time.minute());

Serial.print(F(":"));

if (gps.time.second() < 10) Serial.print(F("0"));

Serial.print(gps.time.second());

Serial.print(F("."));

if (gps.time.centisecond() < 10) Serial.print(F("0"));

Serial.print(gps.time.centisecond());

}

else

{

Serial.print(F("INVALID"));

}

26 November 2025 Update:

Thinking about how to test my GPS-enabled power switch project, I decided to add a cheap LCD display so I could monitor GPS output and power switch status in real time. So, I reached into my parts box and what came out was a pair of ‘Cheap Yellow Display’ modules had been laying around for some time now. As I started working with the display, I realized it wasn’t just a display; the ESP32-2432S028R development board that runs the touch-sensitive display incorporates a ESP32-WROOM-32 module as its main microcontroller. This module features a dual-core Xtensa LX6 processor running at up to 240 MHz, 520 KB SRAM, 448 KB ROM, integrated Wi-Fi (802.11 b/g/n) and Bluetooth (v4.2 BR/EDR and BLE), and 4 MB flash memory. In addition, the board has a standard micro-USB port for power and programming, a USB-C port for power, three 4-pin micro-JST 1.25mm pitch connectors for UART and GPIO, and a 2-pin micro-JST connector for a speaker. Looking at the specs, I realized I could conceivably eliminate the Sparkfun Pro Micro module entirely, and have just the display/CYD connected to the GPS via CN1 (4-pin GPIO/power/GND) and a single GPIO output (plus GND) to run the power switch.

A not-so-insignificant hitch in the getalong for this project was the type of connector required to interface to the sockets on the CYD. It turns out these are ‘micro JST 1.25mm’ connectors, and it took me three tries with Amazon to get the right ones. These are the correct ones.

I was able to easily connect the GPS input to CN1 and use the NMEA simulator to test the code, ported over from the Pro Micro project. However, I hit a roadblock when I went looking for a GPIO port to use as a switch signal to the power switch board; it turns out the display occupies most of the GPIO lines, and the two ports needed for GPS Txd & Rxd took the only remaining output-capable lines (IO35 is available, but it is input-only). Some more research with the help of Grok led me to this project site that describes how to free up ports via hardware mods. There I found directions for freeing up IO21, the port that normally controls display backlight brightness by PWM-ing the backlight LED. This involved removing a surface-mount transistor and re-orienting a TINY! surface-mount 3.9 Ω current-limiting resistor, as shown in the screenshot below:

As you might guess, this took some doing, as these parts are TINY!

But after a couple of false starts, I got it done:

The photo below shows the CYD module with GPS data displayed, and with my newly-liberated IO21 line connected to the power switch board (the illuminated red LED shows that the power switch is ON)

So now we are pretty close to being done; all that is left is to mount the display/controller into its box, mount both the display & power switch boxes onto their custom-printed carrier, and test the whole thing in my truck. The ‘final’ (to the extent that anything I do is actually final) code is shown below:

/*
    Name:       GrokGTU7_GPStoCYD.ino
    Created:	11/24/2025 4:25:32 PM
    Author:     FRANK_XPS_9530\Frank
*/


#include <HardwareSerial.h>
#include <TinyGPSplus.h>
#include <TFT_eSPI.h>  // Or include <lvgl.h> if using LVGL
#include <ElapsedMillis.h>


#define GPS_RX_PIN 27  // CYD GPIO27 as RX (from GT-U7 TX)
#define GPS_TX_PIN 22  // CYD GPIO22 as TX (to GT-U7 RX)
#define DASHCAM_POWER_SWITCH_PIN 21 //See https://github.com/hexeguitar/ESP32_TFT_PIO?tab=readme-ov-file#adding-psram
#define POWER_SWITCH_TRIGGER_PIN 35 //directly manipulate POWER_SWITCH_PIN state for debug
#define GPS_BAUD 9600

#define MAX_SEC_WITHOUT_GPS_DATA 600 //10 min
#define MIN_SPEED_THRESHOLD_MPH 10 //10 mph
#define MAX_TOO_SLOW_TIME_THRESHOLD_SEC 300 //5 min
#define MIN_NUM_SATELLITES_THRESHOLD 4 //4 satellites

//onboard RGB LED
#define PIN_LED_R 4   // Red - use for power OFF
#define PIN_LED_G 16  // Green - use for power ON
#define PIN_LED_B 17  // Blue  - unused


HardwareSerial gpsSerial(2);  // Use UART2
TinyGPSPlus gps;
TFT_eSPI tft = TFT_eSPI();  // For basic display

const int labelX = 10;      // X position for labels
const int valueX = 160;     // X position for values (after label and "=")
const int startY = 10;      // Starting Y for first line
const int lineHeight = 20;  // Spacing between lines (text size 2 needs ~16-20px)
const int valueWidth = 150; // Width of value area to clear (estimate based on max chars)
const int valueHeight = lineHeight;

elapsedMillis MsecSinceTooSlow;
elapsedMillis MsecSinceLastGPSData;

int SecSinceTooSlow = 0;
int SecSinceGPSData = 0;
bool SwitchState = false; //power switch defaults to OFF

void setup() 
{
  Serial.begin(115200);  // For debugging
  gpsSerial.begin(GPS_BAUD, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
  pinMode(DASHCAM_POWER_SWITCH_PIN, OUTPUT);
  pinMode(POWER_SWITCH_TRIGGER_PIN, INPUT_PULLDOWN);
  pinMode(PIN_LED_R, OUTPUT);
  pinMode(PIN_LED_G, OUTPUT);
  pinMode(PIN_LED_B, OUTPUT);

  tft.init();
  tft.setRotation(3);  // Landscape
  tft.invertDisplay(1);  // Fix color inversion
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setTextSize(2);

  // Draw static labels and "=" once
  tft.setCursor(labelX, startY);
  tft.print("Time = ");
  tft.setCursor(labelX, startY +lineHeight);
  tft.print("Sats = ");
  tft.setCursor(labelX, startY + 2 * lineHeight);
  tft.print("Speed = ");
  tft.setCursor(labelX, startY + 3 * lineHeight);
  tft.print("SSTL = ");
  tft.setCursor(labelX, startY + 4 * lineHeight);
  tft.print("SGPSD = ");
  tft.setCursor(labelX, startY + 5 * lineHeight);
  tft.print("Avail = ");
  tft.setCursor(labelX, startY + 6 * lineHeight);
  tft.print("Power = ");

  // Initial values (optional; could be blanks or "N/A")
  updateValues(0, 0, 0, 0, 0.0, 0, 0, 0, 0);

//DEBUG!!
  //digitalWrite(DASHCAM_POWER_SWITCH_PIN, LOW);
  //Serial.printf("POWER_SWITCH_PIN State = %d", digitalRead(DASHCAM_POWER_SWITCH_PIN));
//DEBUG!!
  EnableDashCamPower(false);
}

void loop() 
{
  setLEDColor(LOW, HIGH, HIGH);  // Red ON no GPS data available
  //Serial.printf("In outer loop - LED should be RED\n");

  while (gpsSerial.available() > 0) 
  {
    if (gps.encode(gpsSerial.read())) 
    {
      if (gps.location.isValid()) 
      {
        //Serial.printf("In 'valid' branch - LED should be GREEN\n");
        setLEDColor(HIGH, LOW, HIGH);  // Green ON - valie GPS data available

        updateValues(gps.time.hour(), gps.time.minute(), gps.time.second(),
          gps.satellites.value(), gps.speed.mph(), SecSinceTooSlow, SecSinceGPSData,
          Serial1.available(), SwitchState);

        delay(1000);
      }
      else 
      {
        setLEDColor(HIGH, HIGH, LOW);  //BLUE - GPS data available, but no lock
        //Serial.printf("In 'else' branch - LED should be BLUE\n");
      }
    }

    UpdatePowerSwitchState();
  }

  delay(1000);  // Update every second
}

void updateValues(int hour, int min, int sec, int sats, float speed, int SecSinceTooSlow,
                  int SecSinceValidGPSData, int AvailChars, int SwitchState  ) 
{
  tft.setTextColor(TFT_WHITE, TFT_BLACK);

  // Update Time
  tft.fillRect(valueX, startY, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY);
  tft.printf("%d:%d:%d", hour, min, sec);

  // Update Satellites
  tft.fillRect(valueX, startY + lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY + lineHeight);
  tft.printf("%d", sats);

  // Update Speed
  tft.fillRect(valueX, startY + 2*lineHeight, valueWidth, valueHeight, TFT_BLACK);
  tft.setCursor(valueX, startY + 2* lineHeight);
  tft.printf("%.1f mph", speed);

  // Update SSTL (Since Speed Too Low)
  tft.fillRect(valueX, startY + 3 * lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY + 3 * lineHeight);
  tft.printf("%d Sec", (int)(MsecSinceTooSlow/1000));

  // Update SSGPSD (Seconds Since GPS Data)
  tft.fillRect(valueX, startY + 4 * lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY + 4 * lineHeight);
  tft.printf("%d Sec", (int)(MsecSinceLastGPSData / 1000));

  // Update Characters Remaining
  tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
  tft.setCursor(valueX, startY + 5 * lineHeight);
  tft.printf("%d char", Serial1.available());

  // Update Power Switch State
  tft.fillRect(valueX, startY + 6 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
  tft.setCursor(valueX, startY + 6 * lineHeight);

  int trigger_state = digitalRead(POWER_SWITCH_TRIGGER_PIN);
  String statestr = digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF";
  tft.printf("%s", statestr);
}

void UpdatePowerSwitchState()
{
  //Serial.printf("In UpdatePowerSwitchState()\n");
  //Purpose: Update power switch state 
  //Inputs: 
  // MsecSinceTooSlow = Unsigned Long indicating Msec since speed fell below threshold
  // MsecSinceLastData = Unsigned Long indicating Msec since last GPS data 
  // gps object methods/properties
  //Outputs:
  // Power OFF if GPS data has been missing for more than MAX_SEC_WITHOUT_GPS_DATA
  // Power OFF if [speed < MIN_SPEED_THRESHOLD_MPH OR numsats <=0] AND elapsed time under speed threshold is > MAX_TIME_THRESHOLD_MIN
  //         Otherwise Power ON.
  //Procedure:
  //  Step1: If speed from GPS > threshold, reset MsecSinceTooSlow to zero and Set power to ON
  //  Step2: If (speed < threshold OR numsats < threshold) AND MsecSinceTooSlow > Set power to OFF

//DEBUG:
  //Serial.printf(UpdateTelemStr, gps.time.hour(), gps.time.minute(), gps.time.second(),
  //  gps.speed.knots(), gps.satellites.value(), (unsigned long)MsecSinceTooSlow, (unsigned long)MsecSinceLastGPSData,
  //  Serial1.available(), digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF");
  //  Serial.printf("In UpdatePowerSwitchState().  Speed = %2.1f, Sats = %d\n", gps.speed.mph(), gps.satellites.value());
//DEBUG:

  if (((int)MsecSinceLastGPSData /1000) > MAX_SEC_WITHOUT_GPS_DATA && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)
  {
//DEBUG:
    Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());
//DEBUG:

    EnableDashCamPower(false);//turn dashcam power OFF
    return;
  }

  if ((gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH || (gps.satellites.value() < MIN_NUM_SATELLITES_THRESHOLD))
    && ((int)MsecSinceTooSlow / 1000) > MAX_TOO_SLOW_TIME_THRESHOLD_SEC && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)
  {
//DEBUG:
    Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());
//DEBUG:

    EnableDashCamPower(false);//turn dashcam power OFF
    return;
  }

  if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH && gps.satellites.value() >= MIN_NUM_SATELLITES_THRESHOLD)
  {
    MsecSinceTooSlow = 0;
    MsecSinceLastGPSData = 0;

    //Serial.printf("DashCam Power Switch state = %d\n", digitalRead(DASHCAM_POWER_SWITCH_PIN));

    if (digitalRead(DASHCAM_POWER_SWITCH_PIN) == LOW)
    {
//DEBUG:
      Serial.printf("Powering ON at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());
//DEBUG:

      EnableDashCamPower(true);//turn dashcam power ON
      return;
    }
  }
}

void EnableDashCamPower(bool enable)
{
  digitalWrite(DASHCAM_POWER_SWITCH_PIN, enable);
}

// Helper function to set RGB states
void setLEDColor(int redState, int greenState, int blueState) {
  digitalWrite(PIN_LED_R, redState);
  digitalWrite(PIN_LED_G, greenState);
  digitalWrite(PIN_LED_B, blueState);
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

Name: GrokGTU7_GPStoCYD.ino

Created: 11/24/2025 4:25:32 PM

Author: FRANK_XPS_9530\Frank

#include <HardwareSerial.h>

#include <TinyGPSplus.h>

#include <TFT_eSPI.h> // Or include <lvgl.h> if using LVGL

#include <ElapsedMillis.h>

#define GPS_RX_PIN 27 // CYD GPIO27 as RX (from GT-U7 TX)

#define GPS_TX_PIN 22 // CYD GPIO22 as TX (to GT-U7 RX)

#define DASHCAM_POWER_SWITCH_PIN 21 //See https://github.com/hexeguitar/ESP32_TFT_PIO?tab=readme-ov-file#adding-psram

#define POWER_SWITCH_TRIGGER_PIN 35 //directly manipulate POWER_SWITCH_PIN state for debug

#define GPS_BAUD 9600

#define MAX_SEC_WITHOUT_GPS_DATA 600 //10 min

#define MIN_SPEED_THRESHOLD_MPH 10 //10 mph

#define MAX_TOO_SLOW_TIME_THRESHOLD_SEC 300 //5 min

#define MIN_NUM_SATELLITES_THRESHOLD 4 //4 satellites

//onboard RGB LED

#define PIN_LED_R 4 // Red - use for power OFF

#define PIN_LED_G 16 // Green - use for power ON

#define PIN_LED_B 17 // Blue - unused

HardwareSerial gpsSerial(2); // Use UART2

TinyGPSPlus gps;

TFT_eSPI tft = TFT_eSPI(); // For basic display

const int labelX = 10; // X position for labels

const int valueX = 160; // X position for values (after label and "=")

const int startY = 10; // Starting Y for first line

const int lineHeight = 20; // Spacing between lines (text size 2 needs ~16-20px)

const int valueWidth = 150; // Width of value area to clear (estimate based on max chars)

const int valueHeight = lineHeight;

elapsedMillis MsecSinceTooSlow;

elapsedMillis MsecSinceLastGPSData;

int SecSinceTooSlow = 0;

int SecSinceGPSData = 0;

bool SwitchState = false; //power switch defaults to OFF

void setup()

{

Serial.begin(115200); // For debugging

gpsSerial.begin(GPS_BAUD, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);

pinMode(DASHCAM_POWER_SWITCH_PIN, OUTPUT);

pinMode(POWER_SWITCH_TRIGGER_PIN, INPUT_PULLDOWN);

pinMode(PIN_LED_R, OUTPUT);

pinMode(PIN_LED_G, OUTPUT);

pinMode(PIN_LED_B, OUTPUT);

tft.init();

tft.setRotation(3); // Landscape

tft.invertDisplay(1); // Fix color inversion

tft.fillScreen(TFT_BLACK);

tft.setTextColor(TFT_WHITE);

tft.setTextSize(2);

// Draw static labels and "=" once

tft.setCursor(labelX, startY);

tft.print("Time = ");

tft.setCursor(labelX, startY +lineHeight);

tft.print("Sats = ");

tft.setCursor(labelX, startY + 2 * lineHeight);

tft.print("Speed = ");

tft.setCursor(labelX, startY + 3 * lineHeight);

tft.print("SSTL = ");

tft.setCursor(labelX, startY + 4 * lineHeight);

tft.print("SGPSD = ");

tft.setCursor(labelX, startY + 5 * lineHeight);

tft.print("Avail = ");

tft.setCursor(labelX, startY + 6 * lineHeight);

tft.print("Power = ");

// Initial values (optional; could be blanks or "N/A")

updateValues(0, 0, 0, 0, 0.0, 0, 0, 0, 0);

//DEBUG!!

//digitalWrite(DASHCAM_POWER_SWITCH_PIN, LOW);

//Serial.printf("POWER_SWITCH_PIN State = %d", digitalRead(DASHCAM_POWER_SWITCH_PIN));

//DEBUG!!

EnableDashCamPower(false);

}

void loop()

{

setLEDColor(LOW, HIGH, HIGH); // Red ON no GPS data available

//Serial.printf("In outer loop - LED should be RED\n");

while (gpsSerial.available() > 0)

{

if (gps.encode(gpsSerial.read()))

{

if (gps.location.isValid())

{

//Serial.printf("In 'valid' branch - LED should be GREEN\n");

setLEDColor(HIGH, LOW, HIGH); // Green ON - valie GPS data available

updateValues(gps.time.hour(), gps.time.minute(), gps.time.second(),

gps.satellites.value(), gps.speed.mph(), SecSinceTooSlow, SecSinceGPSData,

Serial1.available(), SwitchState);

delay(1000);

}

else

{

setLEDColor(HIGH, HIGH, LOW); //BLUE - GPS data available, but no lock

//Serial.printf("In 'else' branch - LED should be BLUE\n");

}

UpdatePowerSwitchState();

}

delay(1000); // Update every second

}

void updateValues(int hour, int min, int sec, int sats, float speed, int SecSinceTooSlow,

int SecSinceValidGPSData, int AvailChars, int SwitchState )

{

tft.setTextColor(TFT_WHITE, TFT_BLACK);

// Update Time

tft.fillRect(valueX, startY, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY);

tft.printf("%d:%d:%d", hour, min, sec);

// Update Satellites

tft.fillRect(valueX, startY + lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY + lineHeight);

tft.printf("%d", sats);

// Update Speed

tft.fillRect(valueX, startY + 2*lineHeight, valueWidth, valueHeight, TFT_BLACK);

tft.setCursor(valueX, startY + 2* lineHeight);

tft.printf("%.1f mph", speed);

// Update SSTL (Since Speed Too Low)

tft.fillRect(valueX, startY + 3 * lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY + 3 * lineHeight);

tft.printf("%d Sec", (int)(MsecSinceTooSlow/1000));

// Update SSGPSD (Seconds Since GPS Data)

tft.fillRect(valueX, startY + 4 * lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY + 4 * lineHeight);

tft.printf("%d Sec", (int)(MsecSinceLastGPSData / 1000));

// Update Characters Remaining

tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

tft.setCursor(valueX, startY + 5 * lineHeight);

tft.printf("%d char", Serial1.available());

// Update Power Switch State

tft.fillRect(valueX, startY + 6 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

tft.setCursor(valueX, startY + 6 * lineHeight);

int trigger_state = digitalRead(POWER_SWITCH_TRIGGER_PIN);

String statestr = digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF";

tft.printf("%s", statestr);

}

void UpdatePowerSwitchState()

{

//Serial.printf("In UpdatePowerSwitchState()\n");

//Purpose: Update power switch state

//Inputs:

// MsecSinceTooSlow = Unsigned Long indicating Msec since speed fell below threshold

// MsecSinceLastData = Unsigned Long indicating Msec since last GPS data

// gps object methods/properties

//Outputs:

// Power OFF if GPS data has been missing for more than MAX_SEC_WITHOUT_GPS_DATA

// Power OFF if [speed < MIN_SPEED_THRESHOLD_MPH OR numsats <=0] AND elapsed time under speed threshold is > MAX_TIME_THRESHOLD_MIN

// Otherwise Power ON.

//Procedure:

// Step1: If speed from GPS > threshold, reset MsecSinceTooSlow to zero and Set power to ON

// Step2: If (speed < threshold OR numsats < threshold) AND MsecSinceTooSlow > Set power to OFF

//DEBUG:

//Serial.printf(UpdateTelemStr, gps.time.hour(), gps.time.minute(), gps.time.second(),

// gps.speed.knots(), gps.satellites.value(), (unsigned long)MsecSinceTooSlow, (unsigned long)MsecSinceLastGPSData,

// Serial1.available(), digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF");

// Serial.printf("In UpdatePowerSwitchState(). Speed = %2.1f, Sats = %d\n", gps.speed.mph(), gps.satellites.value());

//DEBUG:

if (((int)MsecSinceLastGPSData /1000) > MAX_SEC_WITHOUT_GPS_DATA && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)

{

//DEBUG:

Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());

//DEBUG:

EnableDashCamPower(false);//turn dashcam power OFF

return;

}

if ((gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH || (gps.satellites.value() < MIN_NUM_SATELLITES_THRESHOLD))

&& ((int)MsecSinceTooSlow / 1000) > MAX_TOO_SLOW_TIME_THRESHOLD_SEC && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)

{

//DEBUG:

Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());

//DEBUG:

EnableDashCamPower(false);//turn dashcam power OFF

return;

}

if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH && gps.satellites.value() >= MIN_NUM_SATELLITES_THRESHOLD)

{

MsecSinceTooSlow = 0;

MsecSinceLastGPSData = 0;

//Serial.printf("DashCam Power Switch state = %d\n", digitalRead(DASHCAM_POWER_SWITCH_PIN));

if (digitalRead(DASHCAM_POWER_SWITCH_PIN) == LOW)

{

//DEBUG:

Serial.printf("Powering ON at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());

//DEBUG:

EnableDashCamPower(true);//turn dashcam power ON

return;

}

void EnableDashCamPower(bool enable)

{

digitalWrite(DASHCAM_POWER_SWITCH_PIN, enable);

}

// Helper function to set RGB states

void setLEDColor(int redState, int greenState, int blueState) {

digitalWrite(PIN_LED_R, redState);

digitalWrite(PIN_LED_G, greenState);

digitalWrite(PIN_LED_B, blueState);

}

Here is a short video of the CYD display and power switch during a driving test in my local development loop. At the start of the video, the GPS module has already detected 8 sats (hard to see), so as soon as the computed speed goes over 5mph, the power switch changes from OFF to ON (red LED on the power switch and ‘ON’ on the CYD.

Unfortunately it was at this point that I discovered that the BUZ76A MOSFET power switch had too high of an Rds (around 4Ω) to actually turn on the camera -bummer!

Some discussion with Grok netted me a low-Rds MOSFET – the AOD424G AOD424 TO252 with Rds in the milli-Ohms. This has a pretty low Vds(max) of around 20V, but this is fine for my needs as it will only be switching 5V. Here’s the new schematic:

01 December 2025 Update:

As things are coming together on the electronics side, I took some time to investigate mounting options in my truck. After fiddling around a bit, I came up with the following possibility:

This would actually work out great, as the PETG plastic boxes wouldn’t be directly exposed to sunlight, and, the display would still be very visible to the driver, and it wouldn’t block vision to my time and temperature display at the top of the radio stack. As a huge bonus, it turns out there is enough slack in the USB power cable to the dash camera that I could plug it directly into the switch box (green in the photo above, if I changed out one of the USB type-C chassis connectors for a USB2/3 type. $7 for 5 connectors from Amazon – delivery in less than a week – yay!

Of course this means I need to change the box design, but that won’t take long.

08 December 2025 Update:

I got my new USB2 connector, and redesigned the box & lid to accommodate it, creating a conundrum in the process.

When I started wiring everything in, I realized I had created a big problem. If I ever wanted to remove the proto-board from the enclosure, I would have to unsolder all the wires to the USB connectors first – UGH!! I thought I could solve this problem by adding connectors in the lines to the USB connectors, but I hated that idea as it just adds complexity (“a connector is a source of trouble between two other sources of trouble”), so I put the whole thing aside to await a better solution. This morning while exercising it came to me – redesign the enclosure so the parting line between box and lid is right at the top edge of the connector modules. Then the proto-board and connectors can be lifted right out after removing the two proto-board mounting screws – genius! Only one small ‘gotcha’ the USB-C connectors will have to be flipped 180𝆩 so the PCB slot is on top rather than on the bottom. Since USB-C connectors are orientation insensitive anyway, this shouldn’t be an issue. Here’s a screenshot of the ‘final’ (as final as anything gets in my world) enclosure, with the connector cutouts moved to the top so removing the lid will allow the PCB – with the connectors still attached – to be lifted out.

Here are some photos of the ‘finished’ product:

After installing it in my truck, I made a couple of test runs around my little development’s circle, and it seems to be working great. Here are some photos showing the installation in my truck:

Here is the ‘finished’ version of the firmware:

/*
    Name:       GrokGTU7_GPStoCYD.ino
    Created:	11/24/2025 4:25:32 PM
    Author:     FRANK_XPS_9530\Frank
*/


#include <HardwareSerial.h>
#include <TinyGPSplus.h>
#include <TFT_eSPI.h>  // Or include <lvgl.h> if using LVGL
#include <ElapsedMillis.h>


#define GPS_RX_PIN 27  // CYD GPIO27 as RX (from GT-U7 TX)
#define GPS_TX_PIN 22  // CYD GPIO22 as TX (to GT-U7 RX)
#define DASHCAM_POWER_SWITCH_PIN 21 //See https://github.com/hexeguitar/ESP32_TFT_PIO?tab=readme-ov-file#adding-psram
#define POWER_SWITCH_TRIGGER_PIN 35 //directly manipulate POWER_SWITCH_PIN state for debug
#define GPS_BAUD 9600

#define MAX_SEC_WITHOUT_GPS_DATA 600 //10 min
#define MIN_SPEED_THRESHOLD_MPH 10 //10 mph
#define MAX_TOO_SLOW_TIME_THRESHOLD_SEC 300 //5 min
#define MIN_NUM_SATELLITES_THRESHOLD 4 //4 satellites

//onboard RGB LED
#define PIN_LED_R 4   // Red - use for power OFF
#define PIN_LED_G 16  // Green - use for power ON
#define PIN_LED_B 17  // Blue  - unused


HardwareSerial gpsSerial(2);  // Use UART2
TinyGPSPlus gps;
TFT_eSPI tft = TFT_eSPI();  // For basic display

const int labelX = 10;      // X position for labels
const int valueX = 160;     // X position for values (after label and "=")
//const int startY = 10;      // Starting Y for first line
const int startY = 110;      // Starting Y for first line 12/10/25 moved down to avoid gearshift stalk
const int lineHeight = 20;  // Spacing between lines (text size 2 needs ~16-20px)
const int valueWidth = 150; // Width of value area to clear (estimate based on max chars)
const int valueHeight = lineHeight;

elapsedMillis MsecSinceTooSlow;
elapsedMillis MsecSinceLastGPSData;

int SecSinceTooSlow = 0;
int SecSinceGPSData = 0;
bool SwitchState = false; //power switch defaults to OFF

void setup() 
{
  Serial.begin(115200);  // For debugging
  gpsSerial.begin(GPS_BAUD, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
  pinMode(DASHCAM_POWER_SWITCH_PIN, OUTPUT);
  pinMode(POWER_SWITCH_TRIGGER_PIN, INPUT_PULLDOWN);
  pinMode(PIN_LED_R, OUTPUT);
  pinMode(PIN_LED_G, OUTPUT);
  pinMode(PIN_LED_B, OUTPUT);

  tft.init();
  tft.setRotation(3);  // Landscape
  tft.invertDisplay(1);  // Fix color inversion
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE);
  tft.setTextSize(2);

  // Draw static labels and "=" once
  tft.setCursor(labelX, startY);
  tft.print("Time = ");
  tft.setCursor(labelX, startY +lineHeight);
  tft.print("Sats = ");
  tft.setCursor(labelX, startY + 2 * lineHeight);
  tft.print("Speed = ");
  tft.setCursor(labelX, startY + 3 * lineHeight);
  tft.print("SSTooSlow = ");
  tft.setCursor(labelX, startY + 4 * lineHeight);
  tft.print("SSGPSD = ");
  //tft.setCursor(labelX, startY + 5 * lineHeight);
  //tft.print("Avail = ");
  //tft.setCursor(labelX, startY + 6 * lineHeight);
  //tft.print("Power = ");
  tft.setCursor(labelX, startY + 5 * lineHeight);
  tft.print("Power = ");

  // Initial values (optional; could be blanks or "N/A")
  //updateValues(0, 0, 0, 0, 0.0, 0, 0, 0, 0);
  EnableDashCamPower(false); //12/10/25 moved above updateValues() to display setup() value
  updateValues(); //12/10/25 now all value extraction done internally

//DEBUG!!
  //digitalWrite(DASHCAM_POWER_SWITCH_PIN, LOW);
  //Serial.printf("POWER_SWITCH_PIN State = %d", digitalRead(DASHCAM_POWER_SWITCH_PIN));
//DEBUG!!
}

void loop() 
{
  setLEDColor(LOW, HIGH, HIGH);  // Red ON no GPS data available
  //Serial.printf("In outer loop - LED should be RED\n");

  while (gpsSerial.available() > 0) 
  {
    if (gps.encode(gpsSerial.read())) 
    {
      if (gps.location.isValid()) 
      {
        //Serial.printf("In 'valid' branch - LED should be GREEN\n");
        setLEDColor(HIGH, LOW, HIGH);  // Green ON - valie GPS data available

        //updateValues(gps.time.hour(), gps.time.minute(), gps.time.second(),
        //  gps.satellites.value(), gps.speed.mph(), SecSinceTooSlow, SecSinceGPSData,
        //  Serial1.available(), SwitchState);
        updateValues(); //12/10/25 now all value extraction done internally

        delay(1000);
      }
      else 
      {
        setLEDColor(HIGH, HIGH, LOW);  //BLUE - GPS data available, but no lock
        //Serial.printf("In 'else' branch - LED should be BLUE\n");
      }
    }

    UpdatePowerSwitchState();
  }

  delay(1000);  // Update every second
}

//void updateValues(int hour, int min, int sec, int sats, float speed, int SecSinceTooSlow,
//                  int SecSinceValidGPSData, int AvailChars, int SwitchState  ) 
//{
//  tft.setTextColor(TFT_WHITE, TFT_BLACK);
//
//  // Update Time
//  tft.fillRect(valueX, startY, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
//  tft.setCursor(valueX, startY);
//  tft.printf("%d:%d:%d", hour, min, sec);
//
//  // Update Satellites
//  tft.fillRect(valueX, startY + lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
//  tft.setCursor(valueX, startY + lineHeight);
//  tft.printf("%d", sats);
//
//  // Update Speed
//  tft.fillRect(valueX, startY + 2*lineHeight, valueWidth, valueHeight, TFT_BLACK);
//  tft.setCursor(valueX, startY + 2* lineHeight);
//  tft.printf("%.1f mph", speed);
//
//  // Update SSTL (Since Speed Too Low)
//  tft.fillRect(valueX, startY + 3 * lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
//  tft.setCursor(valueX, startY + 3 * lineHeight);
//  tft.printf("%d Sec", (int)(MsecSinceTooSlow/1000));
//
//  // Update SSGPSD (Seconds Since GPS Data)
//  tft.fillRect(valueX, startY + 4 * lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
//  tft.setCursor(valueX, startY + 4 * lineHeight);
//  tft.printf("%d Sec", (int)(MsecSinceLastGPSData / 1000));
//
//  // Update Characters Remaining
//  //tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
//  //tft.setCursor(valueX, startY + 5 * lineHeight);
//  //tft.printf("%d char", Serial1.available());
//
//  // Update Power Switch State
//  //tft.fillRect(valueX, startY + 6 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
//  //tft.setCursor(valueX, startY + 6 * lineHeight);
//  tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
//  tft.setCursor(valueX, startY + 5 * lineHeight);
//
//  //int trigger_state = digitalRead(POWER_SWITCH_TRIGGER_PIN);
//  String statestr = digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF";
//  tft.printf("%s", statestr);
//}

//12/10/25 now all value extraction done internally
void updateValues()
{
  tft.setTextColor(TFT_WHITE, TFT_BLACK);

  // Update Time
  tft.fillRect(valueX, startY, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY);
  tft.printf("%d:%d:%d", gps.time.hour(), gps.time.minute(), gps.time.second());

  // Update Satellites
  tft.fillRect(valueX, startY + lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY + lineHeight);
  tft.printf("%d", gps.satellites.value());

  // Update Speed
  tft.fillRect(valueX, startY + 2 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
  tft.setCursor(valueX, startY + 2 * lineHeight);
  tft.printf("%.1f mph", gps.speed.mph());

  // Update SSTL (Since Speed Too Low)
  tft.fillRect(valueX, startY + 3 * lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY + 3 * lineHeight);
  tft.printf("%d Sec", (int)(MsecSinceTooSlow / 1000));

  // Update SSGPSD (Seconds Since GPS Data)
  tft.fillRect(valueX, startY + 4 * lineHeight, valueWidth, valueHeight, TFT_BLACK);  // Clear value area
  tft.setCursor(valueX, startY + 4 * lineHeight);
  tft.printf("%d Sec", (int)(MsecSinceLastGPSData / 1000));

  // Update Characters Remaining
  //tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
  //tft.setCursor(valueX, startY + 5 * lineHeight);
  //tft.printf("%d char", Serial1.available());

  // Update Power Switch State
  //tft.fillRect(valueX, startY + 6 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
  //tft.setCursor(valueX, startY + 6 * lineHeight);
  tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);
  tft.setCursor(valueX, startY + 5 * lineHeight);

  //int trigger_state = digitalRead(POWER_SWITCH_TRIGGER_PIN);
  String statestr = digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF";
  Serial.printf("In UpdateValues(), DASHCAM_POWER_SWITCH_PIN = %d, statestr = %s\n", 
    digitalRead(DASHCAM_POWER_SWITCH_PIN), statestr);
 tft.printf("%s", statestr);
}


void UpdatePowerSwitchState()
{
  //Serial.printf("In UpdatePowerSwitchState()\n");
  //Purpose: Update power switch state 
  //Inputs: 
  // MsecSinceTooSlow = Unsigned Long indicating Msec since speed fell below threshold
  // MsecSinceLastData = Unsigned Long indicating Msec since last GPS data 
  // gps object methods/properties
  //Outputs:
  // Power OFF if GPS data has been missing for more than MAX_SEC_WITHOUT_GPS_DATA
  // Power OFF if [speed < MIN_SPEED_THRESHOLD_MPH OR numsats <=0] AND elapsed time under speed threshold is > MAX_TOO_SLOW_TIME_THRESHOLD_SEC
  //         Otherwise Power ON.
  //Procedure:
  //  Step1: If speed from GPS > threshold, reset MsecSinceTooSlow to zero and Set power to ON
  //  Step2: If (speed < threshold OR numsats < threshold) AND MsecSinceTooSlow > Set power to OFF

//DEBUG:
  //Serial.printf(UpdateTelemStr, gps.time.hour(), gps.time.minute(), gps.time.second(),
  //  gps.speed.knots(), gps.satellites.value(), (unsigned long)MsecSinceTooSlow, (unsigned long)MsecSinceLastGPSData,
  //  Serial1.available(), digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF");
  //  Serial.printf("In UpdatePowerSwitchState().  Speed = %2.1f, Sats = %d\n", gps.speed.mph(), gps.satellites.value());
//DEBUG:

  if (((int)MsecSinceLastGPSData /1000) > MAX_SEC_WITHOUT_GPS_DATA && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)
  {
//DEBUG:
    Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());
//DEBUG:

    EnableDashCamPower(false);//turn dashcam power OFF
    return;
  }

  if ((gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH || (gps.satellites.value() < MIN_NUM_SATELLITES_THRESHOLD))
    && ((int)MsecSinceTooSlow / 1000) > MAX_TOO_SLOW_TIME_THRESHOLD_SEC && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)
  {
//DEBUG:
    Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());
//DEBUG:

    EnableDashCamPower(false);//turn dashcam power OFF
    return;
  }

  if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH && gps.satellites.value() >= MIN_NUM_SATELLITES_THRESHOLD)
  {
    MsecSinceTooSlow = 0;
    MsecSinceLastGPSData = 0;

    //Serial.printf("DashCam Power Switch state = %d\n", digitalRead(DASHCAM_POWER_SWITCH_PIN));

    if (digitalRead(DASHCAM_POWER_SWITCH_PIN) == LOW)
    {
//DEBUG:
      Serial.printf("Powering ON at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());
//DEBUG:

      EnableDashCamPower(true);//turn dashcam power ON
      return;
    }
  }
}

void EnableDashCamPower(bool enable)
{
  digitalWrite(DASHCAM_POWER_SWITCH_PIN, enable);
  Serial.printf("DASHCAM_POWER_SWITCH_PIN = %d\n", digitalRead(DASHCAM_POWER_SWITCH_PIN));
}

// Helper function to set RGB states
void setLEDColor(int redState, int greenState, int blueState) {
  digitalWrite(PIN_LED_R, redState);
  digitalWrite(PIN_LED_G, greenState);
  digitalWrite(PIN_LED_B, blueState);
}

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

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

Name: GrokGTU7_GPStoCYD.ino

Created: 11/24/2025 4:25:32 PM

Author: FRANK_XPS_9530\Frank

#include <HardwareSerial.h>

#include <TinyGPSplus.h>

#include <TFT_eSPI.h> // Or include <lvgl.h> if using LVGL

#include <ElapsedMillis.h>

#define GPS_RX_PIN 27 // CYD GPIO27 as RX (from GT-U7 TX)

#define GPS_TX_PIN 22 // CYD GPIO22 as TX (to GT-U7 RX)

#define DASHCAM_POWER_SWITCH_PIN 21 //See https://github.com/hexeguitar/ESP32_TFT_PIO?tab=readme-ov-file#adding-psram

#define POWER_SWITCH_TRIGGER_PIN 35 //directly manipulate POWER_SWITCH_PIN state for debug

#define GPS_BAUD 9600

#define MAX_SEC_WITHOUT_GPS_DATA 600 //10 min

#define MIN_SPEED_THRESHOLD_MPH 10 //10 mph

#define MAX_TOO_SLOW_TIME_THRESHOLD_SEC 300 //5 min

#define MIN_NUM_SATELLITES_THRESHOLD 4 //4 satellites

//onboard RGB LED

#define PIN_LED_R 4 // Red - use for power OFF

#define PIN_LED_G 16 // Green - use for power ON

#define PIN_LED_B 17 // Blue - unused

HardwareSerial gpsSerial(2); // Use UART2

TinyGPSPlus gps;

TFT_eSPI tft = TFT_eSPI(); // For basic display

const int labelX = 10; // X position for labels

const int valueX = 160; // X position for values (after label and "=")

//const int startY = 10; // Starting Y for first line

const int startY = 110; // Starting Y for first line 12/10/25 moved down to avoid gearshift stalk

const int lineHeight = 20; // Spacing between lines (text size 2 needs ~16-20px)

const int valueWidth = 150; // Width of value area to clear (estimate based on max chars)

const int valueHeight = lineHeight;

elapsedMillis MsecSinceTooSlow;

elapsedMillis MsecSinceLastGPSData;

int SecSinceTooSlow = 0;

int SecSinceGPSData = 0;

bool SwitchState = false; //power switch defaults to OFF

void setup()

{

Serial.begin(115200); // For debugging

gpsSerial.begin(GPS_BAUD, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);

pinMode(DASHCAM_POWER_SWITCH_PIN, OUTPUT);

pinMode(POWER_SWITCH_TRIGGER_PIN, INPUT_PULLDOWN);

pinMode(PIN_LED_R, OUTPUT);

pinMode(PIN_LED_G, OUTPUT);

pinMode(PIN_LED_B, OUTPUT);

tft.init();

tft.setRotation(3); // Landscape

tft.invertDisplay(1); // Fix color inversion

tft.fillScreen(TFT_BLACK);

tft.setTextColor(TFT_WHITE);

tft.setTextSize(2);

// Draw static labels and "=" once

tft.setCursor(labelX, startY);

tft.print("Time = ");

tft.setCursor(labelX, startY +lineHeight);

tft.print("Sats = ");

tft.setCursor(labelX, startY + 2 * lineHeight);

tft.print("Speed = ");

tft.setCursor(labelX, startY + 3 * lineHeight);

tft.print("SSTooSlow = ");

tft.setCursor(labelX, startY + 4 * lineHeight);

tft.print("SSGPSD = ");

//tft.setCursor(labelX, startY + 5 * lineHeight);

//tft.print("Avail = ");

//tft.setCursor(labelX, startY + 6 * lineHeight);

//tft.print("Power = ");

tft.setCursor(labelX, startY + 5 * lineHeight);

tft.print("Power = ");

// Initial values (optional; could be blanks or "N/A")

//updateValues(0, 0, 0, 0, 0.0, 0, 0, 0, 0);

EnableDashCamPower(false); //12/10/25 moved above updateValues() to display setup() value

updateValues(); //12/10/25 now all value extraction done internally

//DEBUG!!

//digitalWrite(DASHCAM_POWER_SWITCH_PIN, LOW);

//Serial.printf("POWER_SWITCH_PIN State = %d", digitalRead(DASHCAM_POWER_SWITCH_PIN));

//DEBUG!!

}

void loop()

{

setLEDColor(LOW, HIGH, HIGH); // Red ON no GPS data available

//Serial.printf("In outer loop - LED should be RED\n");

while (gpsSerial.available() > 0)

{

if (gps.encode(gpsSerial.read()))

{

if (gps.location.isValid())

{

//Serial.printf("In 'valid' branch - LED should be GREEN\n");

setLEDColor(HIGH, LOW, HIGH); // Green ON - valie GPS data available

//updateValues(gps.time.hour(), gps.time.minute(), gps.time.second(),

// gps.satellites.value(), gps.speed.mph(), SecSinceTooSlow, SecSinceGPSData,

// Serial1.available(), SwitchState);

updateValues(); //12/10/25 now all value extraction done internally

delay(1000);

}

else

{

setLEDColor(HIGH, HIGH, LOW); //BLUE - GPS data available, but no lock

//Serial.printf("In 'else' branch - LED should be BLUE\n");

}

UpdatePowerSwitchState();

}

delay(1000); // Update every second

}

//void updateValues(int hour, int min, int sec, int sats, float speed, int SecSinceTooSlow,

// int SecSinceValidGPSData, int AvailChars, int SwitchState )

//{

// tft.setTextColor(TFT_WHITE, TFT_BLACK);

// // Update Time

// tft.fillRect(valueX, startY, valueWidth, valueHeight, TFT_BLACK); // Clear value area

// tft.setCursor(valueX, startY);

// tft.printf("%d:%d:%d", hour, min, sec);

// // Update Satellites

// tft.fillRect(valueX, startY + lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

// tft.setCursor(valueX, startY + lineHeight);

// tft.printf("%d", sats);

// // Update Speed

// tft.fillRect(valueX, startY + 2*lineHeight, valueWidth, valueHeight, TFT_BLACK);

// tft.setCursor(valueX, startY + 2* lineHeight);

// tft.printf("%.1f mph", speed);

// // Update SSTL (Since Speed Too Low)

// tft.fillRect(valueX, startY + 3 * lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

// tft.setCursor(valueX, startY + 3 * lineHeight);

// tft.printf("%d Sec", (int)(MsecSinceTooSlow/1000));

// // Update SSGPSD (Seconds Since GPS Data)

// tft.fillRect(valueX, startY + 4 * lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

// tft.setCursor(valueX, startY + 4 * lineHeight);

// tft.printf("%d Sec", (int)(MsecSinceLastGPSData / 1000));

// // Update Characters Remaining

// //tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

// //tft.setCursor(valueX, startY + 5 * lineHeight);

// //tft.printf("%d char", Serial1.available());

// // Update Power Switch State

// //tft.fillRect(valueX, startY + 6 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

// //tft.setCursor(valueX, startY + 6 * lineHeight);

// tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

// tft.setCursor(valueX, startY + 5 * lineHeight);

// //int trigger_state = digitalRead(POWER_SWITCH_TRIGGER_PIN);

// String statestr = digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF";

// tft.printf("%s", statestr);

//}

//12/10/25 now all value extraction done internally

void updateValues()

{

tft.setTextColor(TFT_WHITE, TFT_BLACK);

// Update Time

tft.fillRect(valueX, startY, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY);

tft.printf("%d:%d:%d", gps.time.hour(), gps.time.minute(), gps.time.second());

// Update Satellites

tft.fillRect(valueX, startY + lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY + lineHeight);

tft.printf("%d", gps.satellites.value());

// Update Speed

tft.fillRect(valueX, startY + 2 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

tft.setCursor(valueX, startY + 2 * lineHeight);

tft.printf("%.1f mph", gps.speed.mph());

// Update SSTL (Since Speed Too Low)

tft.fillRect(valueX, startY + 3 * lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY + 3 * lineHeight);

tft.printf("%d Sec", (int)(MsecSinceTooSlow / 1000));

// Update SSGPSD (Seconds Since GPS Data)

tft.fillRect(valueX, startY + 4 * lineHeight, valueWidth, valueHeight, TFT_BLACK); // Clear value area

tft.setCursor(valueX, startY + 4 * lineHeight);

tft.printf("%d Sec", (int)(MsecSinceLastGPSData / 1000));

// Update Characters Remaining

//tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

//tft.setCursor(valueX, startY + 5 * lineHeight);

//tft.printf("%d char", Serial1.available());

// Update Power Switch State

//tft.fillRect(valueX, startY + 6 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

//tft.setCursor(valueX, startY + 6 * lineHeight);

tft.fillRect(valueX, startY + 5 * lineHeight, valueWidth, valueHeight, TFT_BLACK);

tft.setCursor(valueX, startY + 5 * lineHeight);

//int trigger_state = digitalRead(POWER_SWITCH_TRIGGER_PIN);

String statestr = digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF";

Serial.printf("In UpdateValues(), DASHCAM_POWER_SWITCH_PIN = %d, statestr = %s\n",

digitalRead(DASHCAM_POWER_SWITCH_PIN), statestr);

tft.printf("%s", statestr);

}

void UpdatePowerSwitchState()

{

//Serial.printf("In UpdatePowerSwitchState()\n");

//Purpose: Update power switch state

//Inputs:

// MsecSinceTooSlow = Unsigned Long indicating Msec since speed fell below threshold

// MsecSinceLastData = Unsigned Long indicating Msec since last GPS data

// gps object methods/properties

//Outputs:

// Power OFF if GPS data has been missing for more than MAX_SEC_WITHOUT_GPS_DATA

// Power OFF if [speed < MIN_SPEED_THRESHOLD_MPH OR numsats <=0] AND elapsed time under speed threshold is > MAX_TOO_SLOW_TIME_THRESHOLD_SEC

// Otherwise Power ON.

//Procedure:

// Step1: If speed from GPS > threshold, reset MsecSinceTooSlow to zero and Set power to ON

// Step2: If (speed < threshold OR numsats < threshold) AND MsecSinceTooSlow > Set power to OFF

//DEBUG:

//Serial.printf(UpdateTelemStr, gps.time.hour(), gps.time.minute(), gps.time.second(),

// gps.speed.knots(), gps.satellites.value(), (unsigned long)MsecSinceTooSlow, (unsigned long)MsecSinceLastGPSData,

// Serial1.available(), digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH ? "ON" : "OFF");

// Serial.printf("In UpdatePowerSwitchState(). Speed = %2.1f, Sats = %d\n", gps.speed.mph(), gps.satellites.value());

//DEBUG:

if (((int)MsecSinceLastGPSData /1000) > MAX_SEC_WITHOUT_GPS_DATA && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)

{

//DEBUG:

Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());

//DEBUG:

EnableDashCamPower(false);//turn dashcam power OFF

return;

}

if ((gps.speed.mph() < (double)MIN_SPEED_THRESHOLD_MPH || (gps.satellites.value() < MIN_NUM_SATELLITES_THRESHOLD))

&& ((int)MsecSinceTooSlow / 1000) > MAX_TOO_SLOW_TIME_THRESHOLD_SEC && digitalRead(DASHCAM_POWER_SWITCH_PIN) == HIGH)

{

//DEBUG:

Serial.printf("Powering OFF at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());

//DEBUG:

EnableDashCamPower(false);//turn dashcam power OFF

return;

}

if (gps.speed.mph() >= (double)MIN_SPEED_THRESHOLD_MPH && gps.satellites.value() >= MIN_NUM_SATELLITES_THRESHOLD)

{

MsecSinceTooSlow = 0;

MsecSinceLastGPSData = 0;

//Serial.printf("DashCam Power Switch state = %d\n", digitalRead(DASHCAM_POWER_SWITCH_PIN));

if (digitalRead(DASHCAM_POWER_SWITCH_PIN) == LOW)

{

//DEBUG:

Serial.printf("Powering ON at %02d:%02d:%02d\n", gps.time.hour(), gps.time.minute(), gps.time.second());

//DEBUG:

EnableDashCamPower(true);//turn dashcam power ON

return;

}

void EnableDashCamPower(bool enable)

{

digitalWrite(DASHCAM_POWER_SWITCH_PIN, enable);

Serial.printf("DASHCAM_POWER_SWITCH_PIN = %d\n", digitalRead(DASHCAM_POWER_SWITCH_PIN));

}

// Helper function to set RGB states

void setLEDColor(int redState, int greenState, int blueState) {

digitalWrite(PIN_LED_R, redState);

digitalWrite(PIN_LED_G, greenState);

digitalWrite(PIN_LED_B, blueState);

}

15 December 2025 Update:

Now that things seem to be working, I’m trying to clean up the installation a bit. Here are some photos showing some of the current temporary installation details:

Overall installation layout, from power source to GPS antenna

I also took the opportunity to measure the current load with the display and switch box electronics powered, but with the dashcam OFF. This is important, as this load determines how long the battery will last in this configuration. I used my handy-dandy USB2 ‘Charger Doctor’ pass-through device that alternately displays output voltage and current. As shown in the following photo, the ‘OFF’ current draw is about 0.19A.

Showing current draw in the ‘dashcam OFF’ configuration. The meter shows 0.19A (190 mA).

The 12V car battery in this truck is a ‘DieHard’ AGM Technology battery as shown below:

So I asked Grok how long this battery would last with the dashcam OFF, but the display (and it’s imbedded MCU) still powered up with a 0.2A draw, and this is what I got back:

251215 Grok Re DieHard Battery Download

So almost a month – that sounds about right, and it means I wouldn’t have had a dead battery at 2 am when my wife and I got home from last summer after a week at the 2025 Senior Games in Des Moines, Iowa!

Just as a sidenote, I have a ‘premium’ subscription to X not because I think X posts are worthwhile, but because Grok *IS* worthwhile. As a long-time electrical engineer, I was familiar with the property of lead-acid batteries that extended the discharge time under very small loads compared to the normal A-Hr rating, but I had never heard of the ‘Peukert effect’, which allows for a much better estimate of discharge times for low loads. This took Grok about 30 seconds – what a *great* engineering assistant!

And, speaking of Grok, I also asked him (it?) for the name and purpose of the ‘cubbyhole’ I’m using as the mounting cavity for the dashcam switch project. Turns out this was probably originally intended for an ashtray, and then repurposed as the mounting cavity for a trailer brake module. And, he also came up with dimensions, which should allow me to design a cradle that will fit snugly in the hole and keep the dashcam electronics from sliding around. We’ll see!

17 December 2025 Update:

A couple of interesting things happened in the last two days. The first thing is I have been cursing at the maker of the SuperONE cigar-lighter automobile power adapter for providing a USB-C retractable cable that doesn’t deliver +5V to power my dashcam switch module.

Because of this, I gave the product a ‘1’ review and described my methodology in determining that the retractable USB-C cable just wasn’t providing +5V. This got the company’s attention, and they offered to send me a new one, free of charge. I said, ‘OK’ but if the new one doesn’t work either, then my review will be edited to say that. Well, the new one arrived, and the same thing happened – no +5V to my project box. However, this time I was smart enough to try plugging the cable into my BT headset, and voila – it started charging the headset! Hmm… After some back and forth with Grok I learned that a ‘Power Delivery’ (PD) device doesn’t deliver power to a connection until it is asked to do so. The BT headset did, my cheap USB-C chassis receptacle didn’t – oops! So, I wound up editing my review from a 1 to a 5 and explained why; I suspect that will make the SuperONE folks a lot happier ;). Also, I ordered a different USB-C receptacle – one that has the smarts to request power from the USB-C cable.

The second thing that happened is I’m trying to figure out how to mount my dashcam power switch project into the ashtray/trailer brake ‘cubbyhole’ (see the photos above). And because I have not only one – but two 3D printers it naturally involves 3D printed parts. I designed and printed a shape that fits nicely into the cavity, and then I’ll need to figure out exactly how the combined assembly can be fastened into the cavity in a way that can be removed easily, if necessary, but also won’t just fall out at the slightest provocation.

1;8 December 2025:

After getting the enclosure issues resolved, I turned my attention to the GPS module, which is kind of hanging around on the shelf above the central audio panel. I’d like to build a small enclosure just to hide it away, leaving only the small GPS antenna itself and the even smaller RF cable leading down to the GPS module visible.

22 December 2025 Update:

I received and installed the new PD-enabled USB-C module, and it went in OK, with the usual curse-words and adaptations:

I printed up a small right-angle adaptor to mate the holes on the USB-C module with the holes on the box, but I had to mount the module on the inside wall – I decided not to make a new box just for this issue. So now The PD-enabled retractable cable on the cigar-lighter adaptor delivers 5V to the module and life is good!

Here’s the ‘final’ installation (nothing is every really ‘final’ in the Paynter world)

23 January 2026 Update:

I’ve been using the DashCam power switch for about a month now, and it seems to be working great. A couple of weeks ago I modifed the ‘Too Slow’ and ‘No Data’ readouts to change the unit from ‘sec’ to ‘min’ and ‘hours’ as appropriate; trying to figure out what ‘83520 sec’ meant was too hard for my brain, so I changed the code so this number would read ‘23.2 hours’

I also noticed that when I drive into my earth-sheltered garage, I get some strange status display readings. The number of sats will go to zero, but the clock time will often stay running (and stay accurate). Also, since the clock is active, the speed value will actually start to climb (divide by-almost-zero?) and so the speed criteria for keeping the power ON will be met. I think the way to handle this is to have a separate block in the logic for sats < min_satellites to force everything but the ‘Too Slow’ and ‘No Data’ timers to show “n/a”

26 January 2026 Update:

The dashcam switch is working well, but I’m not happy with the physical installation. I thought I had a great idea by putting it into the space originally designed for an ashtray, but it turns out not to be such a hot idea. The gear selector lever blocks out a significant amount of the display in ‘D’, and looking down at the display (and peering around the gear shifter lever) isn’t great. So, I decided to ‘re-imagine’ the entire installation.

The original installation features separate boxes for the display and the switch, which wastes a LOT of space. Starting from a clean sheet, I laid out the the switch circuitry and GPS module on a plug board, as shown in the following photo.

Required hardware for complete GPS-to-CYD implementation.

As shown above, the power input (USB-C), power output (USB2), switch (AOD424 MOSFET) and GT-U7 GPS modules easily fit into the footprint of the CYD display. I should be able to put everything into one box with the footprint of the CYD and sufficient depth to accommodate the switch & GPS hardware

However, the display area where I want to install the new version of the dash cam features a deeply recessed display area, and a complex profile. So, I needed to somehow model the profile so the box design would work. As the following photos show I used a combination of ‘cut & try’ for the first rough estimate, and then a contour gauge to get an estimate of the rest of the profile.

Then I used my 3D printer magic to print out successive profile estimates until I got reasonably close to the actual profile. This then can be used for the actual box design. The following photo shows the progression from left to right.

After getting the profile reasonably close, I printed a prototype box by extruding out the profile a few tens of millimeters, and got this result:

After a few more iterations I printed a full-size box:

Here’s a photo showing the old and new installations:

The above photo shows the old and new installations. The new installation will actually move to the left a bit as shown below, as it will incorporate the GPS module internally.

This will still hide the truck’s clock display, but since the new unit will also display time, that’s not a loss of functionality.

The next step was to disassemble the old installation to reuse the parts:

As can be seen from the above photo, there is quite a bit of wasted space in the power switch box, and I also figured out that one of the two USB-C chassis-mount connectors can also be removed, as the power connection from the power-switch circuit to the display will be routed internally – no need for a separate USB-C cable. I breadboarded the power switch circuit again, this time with compactness in mind, and wound up with this:

and then transferred the parts to a small piece of DIY circuit board. Then the whole thing fits nicely into the new enclosure, as shown in the following photos:

After more than a few hours of frustration due to the lack of a reliable GPS signal in the house, I think I finally got everything running properly, and tested it on my subdivision loop:

Here’s the updated schematic, with a missing ground wire added and with signal/control GPIO numbers added:

Stay tuned,

Frank

25 June 2026 Update:

27 June 2026 Update:

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

08 August 2026 Update:

Board.txt (lives in the same folder as the project & .ino file):

C:\Users\Frank\Documents\Robot_Projects\my_vision_robot\software\Wifi_OTA\teensy\Wifi_OTA\PostBuild_OTA.py:

C:\Users\Frank\Documents\Robot_Projects\my_vision_robot\software\Wifi_OTA\pi5\Wifi_OTA.py:

Wall-E2 gets too close to an obstacle in front

Wall-E2 gets too close to the currently-tracked wall during tracking operations

Wall-E2 senses an upcoming wall while tracking the current wall

Wall-E2 hits something that causes it to get stuck and stop moving forward but the obstacle doesn’t register on the forward sensor

Wall-E2 gets too close to an obstacle to the rear

08 April 2026 Update:

16 April 2026 Update:

22 April 2026 Update:

23 April 2026 Update:

25 April 2026 Update:

27 April 2026 Update:

02 May 2026 Update:

04 May 2026 Update:

05 May 2026 Update:

22 March 2026 Update:

27 March 2026 Update:

01 November 2025 Update:

02 November 2025 Update:

03 November 2025 Update:

07 November 2025 Update:

12 November 2025 Update:

26 November 2025 Update:

01 December 2025 Update:

08 December 2025 Update:

15 December 2025 Update:

17 December 2025 Update:

1;8 December 2025:

22 December 2025 Update:

23 January 2026 Update:

26 January 2026 Update: