/*
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");
}
}