Absolutely - this is a good candidate for a clean v3 protocol with the same handshake pattern on both controllers.

Below I’ll give you:

1. A shared ARC JavaScript helper
2. A Teensy firmware example in C++ / Teensyduino
3. A Mega firmware example in C++ / Arduino
4. A step-by-step logic flow for both ARC manager skills
5. What ARC sends vs what firmware must handle
6. A few notes on how to keep both manager scripts nearly identical

I’m going to keep the protocol simple and consistent:

Proposed v3 Protocol

Text line format for startup / status

Use newline-terminated ASCII messages:

• HELLO
• READY,<device>,<version>
• STATUS?
• STATUS,<state>,<details>
• ACK,<message>
• NAK,<reason>

Example flow

ARC -> HELLO
Teensy -> READY,Teensy,v3
ARC -> STATUS?
Teensy -> STATUS,IDLE,IR=0,SERVO=90
ARC -> START
Teensy -> ACK,START

For the Mega, you can keep the same startup flow:

ARC -> HELLO
Mega -> READY,Mega,v3
ARC -> STATUS?
Mega -> STATUS,READY,MOTORS=4

1) Shared ARC JavaScript Helper

This helper can be reused by both managers so they look almost identical.

var UART_INDEX = 0;
var UART_BAUD = 115200;

var deviceName = "DEVICE";
var deviceReady = false;
var lastHelloMs = 0;
var lastStatusMs = 0;
var running = true;

function nowMs() {
  return (new Date()).getTime();
}

function checksum(payload) {
  var sum = 0;
  for (var i = 0; i < payload.length; i++) {
    sum = (sum + payload.charCodeAt(i)) & 0xFF;
  }
  return sum;
}

function toHex2(value) {
  var hex = value.toString(16).toUpperCase();
  if (hex.length < 2) hex = "0" + hex;
  return hex;
}

function buildPacket(payload) {
  var chk = checksum(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

function sendLine(line) {
  print("TX: " + line);
  UART.hardwareUartWriteString(UART_INDEX, line + "\n");
}

function sendPacket(payload) {
  var packet = buildPacket(payload);
  print("TX: " + packet.trim());
  UART.hardwareUartWriteString(UART_INDEX, packet);
}

function requestHello() {
  sendLine("HELLO");
  lastHelloMs = nowMs();
}

function requestStatus() {
  sendLine("STATUS?");
  lastStatusMs = nowMs();
}

function sendStart() {
  sendLine("START");
}

function sendStop() {
  sendLine("STOP");
}

function handleLine(line) {
  var parts = line.split(",");
  var head = parts[0];

  if (head == "READY") {
    deviceReady = true;
    print(deviceName + " ready: " + line);
    return;
  }

  if (head == "ACK") {
    print("ACK: " + line);
    return;
  }

  if (head == "NAK") {
    print("NAK: " + line);
    return;
  }

  if (head == "STATUS") {
    print("STATUS: " + line);
    return;
  }

  print("Unknown line: " + line);
}

function readIncomingLines() {
  var available = UART.hardwareUartAvailable(UART_INDEX);
  while (available > 0) {
    var line = UART.hardwareUartReadLine(UART_INDEX);
    if (line != null && line.length > 0) {
      handleLine(line.trim());
    }
    available = UART.hardwareUartAvailable(UART_INDEX);
  }
}

function maintainHandshake() {
  var now = nowMs();

  if (!deviceReady && (now - lastHelloMs) > 2000) {
    print(deviceName + " not ready yet - retrying HELLO");
    requestHello();
  }

  if (deviceReady && (now - lastStatusMs) > 5000) {
    requestStatus();
  }
}

Notes

• This assumes you want the ARC side to use plain line-based text for handshake/status.
• If you still want your binary packet protocol for motor data, this helper can be extended to support both.
• If you want, I can also convert this into a proper ARC Script Collection layout so one manager calls the shared helper file logic.

2) Teensy Firmware v3 Example

This is a clean Teensyduino example implementing:

• HELLO
• READY
• STATUS?
• STATUS,...
• START
• STOP
• ACK
• NAK

#include <Arduino.h>

static const uint32_t BAUD = 115200;

String rxLine = "";
bool systemReady = false;
bool running = false;

int servoPos = 90;
int irValue = 0;

void sendLine(const String &line) {
  Serial.println(line);
}

void sendReady() {
  sendLine("READY,Teensy,v3");
}

void sendStatus() {
  String s = "STATUS,";
  s += (running ? "RUNNING" : "IDLE");
  s += ",IR=";
  s += String(irValue);
  s += ",SERVO=";
  s += String(servoPos);
  sendLine(s);
}

void sendAck(const String &msg) {
  sendLine("ACK," + msg);
}

void sendNak(const String &reason) {
  sendLine("NAK," + reason);
}

void handleCommand(String line) {
  line.trim();
  line.toUpperCase();

  if (line == "HELLO") {
    systemReady = true;
    sendReady();
    return;
  }

  if (line == "STATUS?") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    sendStatus();
    return;
  }

  if (line == "START") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    running = true;
    sendAck("START");
    return;
  }

  if (line == "STOP") {
    running = false;
    sendAck("STOP");
    return;
  }

  sendNak("UNKNOWN_CMD");
}

void readSerialLines() {
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\n') {
      if (rxLine.length() > 0) {
        handleCommand(rxLine);
        rxLine = "";
      }
    } else if (c != '\r') {
      rxLine += c;
    }
  }
}

void setup() {
  Serial.begin(BAUD);
  delay(500);
  sendReady();
}

void loop() {
  readSerialLines();

  if (running) {
    irValue = analogRead(A0);
    // servo control logic would go here
  }
}

What this Teensy firmware does

• Responds to HELLO with READY,Teensy,v3
• Responds to STATUS? with current runtime info
• Responds to START / STOP
• Keeps the handshake logic simple and predictable

If your Teensy really is on UART2, you’ll likely want to replace Serial with Serial2 and set pin assignments accordingly. If that’s your actual wiring, tell me which Teensy model you’re using and I’ll rewrite it for the correct UART port.

3) Mega Firmware v3 Example

This is the matching Arduino Mega version using Serial1, Serial2, etc.
If your Mega is physically connected to the EZB UART0, then the firmware side depends on which hardware serial port you wired to the EZB.

Here’s the same handshake style:

#include <Arduino.h>

static const uint32_t BAUD = 115200;

String rxLine = "";
bool systemReady = false;
bool motorsEnabled = false;
uint32_t stepCount = 0;

void sendLine(const String &line) {
  Serial.println(line);
}

void sendReady() {
  sendLine("READY,Mega,v3");
}

void sendStatus() {
  String s = "STATUS,";
  s += (motorsEnabled ? "RUNNING" : "READY");
  s += ",STEPS=";
  s += String(stepCount);
  sendLine(s);
}

void sendAck(const String &msg) {
  sendLine("ACK," + msg);
}

void sendNak(const String &reason) {
  sendLine("NAK," + reason);
}

void stopAllMotors() {
  motorsEnabled = false;
  // add your motor driver stop code here
}

void startAllMotors() {
  motorsEnabled = true;
  // add your motor driver enable code here
}

void handleCommand(String line) {
  line.trim();
  line.toUpperCase();

  if (line == "HELLO") {
    systemReady = true;
    sendReady();
    return;
  }

  if (line == "STATUS?") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    sendStatus();
    return;
  }

  if (line == "START") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    startAllMotors();
    sendAck("START");
    return;
  }

  if (line == "STOP") {
    stopAllMotors();
    sendAck("STOP");
    return;
  }

  sendNak("UNKNOWN_CMD");
}

void readSerialLines() {
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\n') {
      if (rxLine.length() > 0) {
        handleCommand(rxLine);
        rxLine = "";
      }
    } else if (c != '\r') {
      rxLine += c;
    }
  }
}

void setup() {
  Serial.begin(BAUD);
  delay(500);
  sendReady();
}

void loop() {
  readSerialLines();

  if (motorsEnabled) {
    stepCount++;
    // stepper motion control logic goes here
  }
}

4) Step-by-Step Manager Logic Flow

A. Teensy Manager Logic Flow

ARC side

1. Open UART2 at 115200.
2. Send HELLO.
3. Wait for READY,Teensy,v3.
4. Send STATUS?.
5. Wait for STATUS,....
6. When ARC wants to start the subsystem, send START.
7. Teensy replies ACK,START.
8. ARC may periodically poll STATUS?.
9. When shutting down or disabling, ARC sends STOP.
10. Teensy replies ACK,STOP.

Teensy firmware side

1. Wait for serial data.
2. On HELLO, mark itself ready and reply READY,Teensy,v3.
3. On STATUS?, send runtime state.
4. On START, enable servo/sensor loop and reply ACK,START.
5. On STOP, disable runtime behavior and reply ACK,STOP.
6. If unknown command, reply NAK,UNKNOWN_CMD.

B. Mega Manager Logic Flow

ARC side

1. Open UART0 at 115200.
2. Send HELLO.
3. Wait for READY,Mega,v3.
4. Send STATUS?.
5. Wait for STATUS,....
6. Send motion/stepper commands when needed.
7. Optionally send START and STOP for lifecycle control.
8. Keep polling status or reading acknowledgements.

Mega firmware side

1. Wait for serial input.
2. On HELLO, reply READY,Mega,v3.
3. On STATUS?, report system state.
4. On START, enable motor logic.
5. On STOP, disable all motors.
6. On invalid command, return NAK,....

5) What ARC Sends vs What Firmware Must Handle

ARC JavaScript should send

For both controllers:

• HELLO
• STATUS?
• START
• STOP

Optional:

• motion-specific commands
• sensor read requests
• configuration messages

Firmware should handle

For both controllers:

• HELLO READY,...
• STATUS? STATUS,...
• START ACK,START
• STOP ACK,STOP
• unknown input NAK,UNKNOWN_CMD

6) How to Make Both ARC Managers Almost Identical

A good pattern is:

• one shared helper
• one tiny Teensy manager
• one tiny Mega manager

Each manager only defines:

• UART_INDEX
• deviceName
• device-specific send/receive handlers

That way your manager scripts become mostly:

UART_INDEX = 2;
deviceName = "Teensy";

or

UART_INDEX = 0;
deviceName = "Mega";

Everything else stays shared.

7) Important ARC Note

Your current Mega manager uses a binary packet protocol with headers, command IDs, payload lengths, and checksums. That is fine - but it is different from the Teensy text protocol you posted.

For v3, I strongly recommend choosing one protocol style for both devices:

Option A: Text-only protocol

Best for:

• startup
• debug
• easier firmware maintenance

Option B: Binary protocol

Best for:

• compact commands
• less serial bandwidth
• structured data

Best compromise

Use:

• text for handshake and status
• binary for high-rate control/data

That gives you clean startup plus efficient runtime control.

8) Recommended Next Step

If you want, I can do the next pass and turn this into:

1. A finalized v3 text protocol
2. A fully refactored ARC JavaScript shared helper
3. A Mega firmware sketch using your current command IDs
4. A Teensy firmware sketch that matches the same binary framing
5. A message table mapping every command and response

That would let you drop the code in with minimal changes.

Also, for code restructuring like this, the ARC AI Assistant built into ARC can be very helpful for rewriting and refactoring your script directly in the editor:
https://synthiam.com/Products/AI-Assistant

Quick question so I can make this exact:

Do you want v3 to be:

1. Text-based only for both Teensy and Mega, or
2. Text handshake + binary runtime payloads matching your current Mega packet style?

If you answer that, I can produce the exact v3 code set in the format you can paste straight into ARC and Arduino/Teensyduino.

@Athena. Thank you. Can you do your recommendations?

Recommended Next Step

If you want, I can do the next pass and turn this into:

1. A finalized v3 text protocol
2. A fully refactored ARC JavaScript shared helper
3. A Mega firmware sketch using your current command IDs
4. A Teensy firmware sketch that matches the same binary framing
5. A message table mapping every command and response

That would let you drop the code in with minimal changes

Quick question so I can make this exact:

Do you want v3 to be:

1. Text-based only for both Teensy and Mega, or
2. Text handshake + binary runtime payloads matching your current Mega packet style?

ANSWER: Please do option #2. Text handshake + binary runtime payloads If you answer that, I can produce the exact v3 code set in the format you can paste straight into ARC and Arduino/Teensyduino.

Finally, to be clear on UART connections for the code rewrite:

UART used between

• Teensy 4.1 (TX8 pin35) connected to EZBv4 (RX2 pin19).
• Teensy 4.1 (RX8 pin34) connected to EZBv4 (TX2 pin18).

UART used between

• Mega (TX3 pin14) connected to EZBv4 (RX0).
• Mega (RX3 pin15) connected to EZBv4 (TX0).

Thank you. Jack

Excellent - option #2 is the right choice here. That gives you a clean text handshake for startup/debug and binary payloads for the actual runtime control, which is a great fit for ARC and the two microcontrollers.

Below is a complete v3 recommendation set:

• One shared ARC JavaScript helper
• One Teensy 4.1 firmware sketch
• One Mega firmware sketch
• A command/response message table
• Step-by-step flow for each manager
• Pin mapping notes for your exact wiring

v3 Protocol Design

High-level behavior

Startup / handshake

Use text lines:

• HELLO
• READY,<device>,v3
• STATUS?
• STATUS,...
• ACK,<cmd>
• NAK,<reason>

Runtime control

Use binary framed packets with your existing Mega-style format:

• 0xA5 header
• cmd
• len
• payload[len]
• checksum

This lets ARC quickly send structured data for motor/sensor control, while the firmware still has a very readable startup handshake.

1) Shared ARC JavaScript Helper

This helper is designed so your Teensy manager and Mega manager can share nearly all logic.

Shared framing rules

• Text handshake for startup
• Binary packets for runtime
• Same parser model for both

Shared helper code

var UART_INDEX = 0;
var UART_BAUD = 115200;
var DEVICE_NAME = "DEVICE";

var deviceReady = false;
var lastHelloMs = 0;
var lastStatusMs = 0;
var running = true;

var UART_HEADER = 0xA5;

// -------------------------
// Utility
// -------------------------
function nowMs() {
  return (new Date()).getTime();
}

function safeInt(v, def) {
  var n = parseInt(v);
  if (isNaN(n)) return def;
  return n;
}

function clamp(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

function toHex2(value) {
  var hex = "0123456789ABCDEF";
  return hex.charAt((value >> 4) & 0x0F) + hex.charAt(value & 0x0F);
}

// -------------------------
// Text handshake helpers
// -------------------------
function sendText(line) {
  print("TX: " + line);
  UART.hardwareUartWriteString(UART_INDEX, line + "\n");
}

function requestHello() {
  sendText("HELLO");
  lastHelloMs = nowMs();
}

function requestStatus() {
  sendText("STATUS?");
  lastStatusMs = nowMs();
}

function sendStart() {
  sendText("START");
}

function sendStop() {
  sendText("STOP");
}

// -------------------------
// Binary packet helpers
// -------------------------
function checksumPacket(cmd, payload) {
  var sum = (cmd ^ payload.length) & 0xFF;
  for (var i = 0; i < payload.length; i++) sum ^= (payload[i] & 0xFF);
  return sum & 0xFF;
}

function sendBinaryPacket(cmd, payload) {
  var packet = [];
  packet.push(UART_HEADER);
  packet.push(cmd & 0xFF);
  packet.push(payload.length & 0xFF);
  for (var i = 0; i < payload.length; i++) packet.push(payload[i] & 0xFF);
  packet.push(checksumPacket(cmd, payload));
  UART.hardwareUartWrite(UART_INDEX, packet);
  print("TX BIN: header=0xA5 cmd=" + cmd + " len=" + payload.length);
}

function u16ToBytes(v) {
  v = v & 0xFFFF;
  return [v & 0xFF, (v >> 8) & 0xFF];
}

function u32ToBytes(v) {
  v = v >>> 0;
  return [
    v & 0xFF,
    (v >> 8) & 0xFF,
    (v >> 16) & 0xFF,
    (v >> 24) & 0xFF
  ];
}

function bytesToU16LE(b0, b1) {
  return ((b0 & 0xFF) | ((b1 & 0xFF) << 8)) >>> 0;
}

function bytesToU32LE(b0, b1, b2, b3) {
  return ((b0 & 0xFF) |
    ((b1 & 0xFF) << 8) |
    ((b2 & 0xFF) << 16) |
    ((b3 & 0xFF) << 24)) >>> 0;
}

// -------------------------
// Text line parser
// -------------------------
function handleTextLine(line) {
  line = line.trim();
  if (line.length == 0) return;

  var parts = line.split(",");
  var head = parts[0];

  if (head == "READY") {
    deviceReady = true;
    print(DEVICE_NAME + " READY: " + line);
    return;
  }

  if (head == "ACK") {
    print(DEVICE_NAME + " ACK: " + line);
    return;
  }

  if (head == "NAK") {
    print(DEVICE_NAME + " NAK: " + line);
    return;
  }

  if (head == "STATUS") {
    print(DEVICE_NAME + " STATUS: " + line);
    return;
  }

  print(DEVICE_NAME + " TEXT: " + line);
}

// -------------------------
// Binary parser
// -------------------------
var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

function handleBinaryPacket(cmd, payload) {
  print(DEVICE_NAME + " BIN RX cmd=" + cmd + " len=" + payload.length);
  // Override this in the device-specific script
}

function processIncoming() {
  var available = UART.hardwareUartAvailable(UART_INDEX);

  while (available > 0) {
    var b = UART.hardwareUartRead(UART_INDEX, 1)[0] & 0xFF;
    available--;

    if (rxState == 0) {
      if (b == UART_HEADER) {
        rxState = 1;
      } else {
        // Treat as text byte start if firmware sends text without header
        // In practice, startup messages are text lines, so this is ignored here.
      }
      continue;
    }

    if (rxState == 1) {
      rxCmd = b;
      rxChecksum = (rxCmd ^ 0) & 0xFF;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b & 0xFF;
      rxChecksum ^= rxLen;
      rxPayload = [];
      rxIndex = 0;
      rxState = (rxLen == 0) ? 4 : 3;
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b & 0xFF);
      rxChecksum ^= (b & 0xFF);
      rxIndex++;
      if (rxIndex >= rxLen) rxState = 4;
      continue;
    }

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        handleBinaryPacket(rxCmd, rxPayload);
      } else {
        print(DEVICE_NAME + " RX checksum error. cmd=" + rxCmd + " expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

      rxState = 0;
      rxCmd = 0;
      rxLen = 0;
      rxIndex = 0;
      rxPayload = [];
      rxChecksum = 0;
      continue;
    }
  }
}

// -------------------------
// Handshake maintenance
// -------------------------
function maintainHandshake() {
  var now = nowMs();

  if (!deviceReady && (now - lastHelloMs) > 2000) {
    print(DEVICE_NAME + " not ready - retry HELLO");
    requestHello();
  }

  if (deviceReady && (now - lastStatusMs) > 5000) {
    requestStatus();
  }
}

2) Teensy 4.1 Firmware Sketch

This version uses:

• Serial8 for the UART to EZB
• Pin 35 = TX8
• Pin 34 = RX8

That matches your wiring:

• Teensy TX8 pin35 -> EZB RX2 pin19
• Teensy RX8 pin34 -> EZB TX2 pin18

Teensy firmware

#include <Arduino.h>

static const uint32_t BAUD = 115200;
static const uint8_t UART_HEADER = 0xA5;

// Teensy 4.1 UART8 wiring
// TX8 = pin 35
// RX8 = pin 34
#define LINK_SERIAL Serial8

String rxLine = "";
bool systemReady = false;
bool running = false;

// Example device state
int servoPos = 90;
int irValue = 0;

// -------------------------
// Binary command IDs
// -------------------------
enum CommandId : uint8_t {
  CMD_STOP_MOTORS     = 0x01,
  CMD_ACTUAL_BEARING  = 0x03,
  CMD_RIGHT_PIVOT     = 0x04,
  CMD_LEFT_PIVOT      = 0x05,
  CMD_WAYPOINT_DATA   = 0x06
};

// -------------------------
// Binary response IDs
// -------------------------
enum ResponseId : uint8_t {
  RSP_NAV_ACK         = 0x10,
  RSP_TOTAL_STEPS     = 0x11,
  RSP_DESIRED_BEARING = 0x12,
  RSP_OBS_SCAN_REQ    = 0x13,
  RSP_READY           = 0x14,
  RSP_STATUS          = 0x15,
  RSP_ACK             = 0x16,
  RSP_NAK             = 0x17
};

void sendText(const String& s) {
  LINK_SERIAL.println(s);
}

void sendReady() {
  sendText("READY,Teensy,v3");
}

void sendStatus() {
  String s = "STATUS,";
  s += (running ? "RUNNING" : "IDLE");
  s += ",IR=";
  s += String(irValue);
  s += ",SERVO=";
  s += String(servoPos);
  sendText(s);
}

void sendAck(const String& msg) {
  sendText("ACK," + msg);
}

void sendNak(const String& reason) {
  sendText("NAK," + reason);
}

uint8_t checksumPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  uint8_t sum = (cmd ^ len);
  for (uint8_t i = 0; i < len; i++) {
    sum ^= payload[i];
  }
  return sum;
}

void sendBinary(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  LINK_SERIAL.write(UART_HEADER);
  LINK_SERIAL.write(cmd);
  LINK_SERIAL.write(len);
  for (uint8_t i = 0; i < len; i++) {
    LINK_SERIAL.write(payload[i]);
  }
  LINK_SERIAL.write(checksumPacket(cmd, payload, len));
}

void stopAll() {
  running = false;
  // stop servo / outputs / any motion
}

void startAll() {
  running = true;
  // enable servo / output control
}

void handleTextCommand(String line) {
  line.trim();
  line.toUpperCase();

  if (line == "HELLO") {
    systemReady = true;
    sendReady();
    return;
  }

  if (line == "STATUS?") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    sendStatus();
    return;
  }

  if (line == "START") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    startAll();
    sendAck("START");
    return;
  }

  if (line == "STOP") {
    stopAll();
    sendAck("STOP");
    return;
  }

  sendNak("UNKNOWN_CMD");
}

// -------------------------
// Binary runtime packet handling
// -------------------------
void handleBinaryPacket(uint8_t cmd, uint8_t* payload, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopAll();
      sendBinary(RSP_ACK, (uint8_t*)"S", 1);
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        uint16_t bearing100 = (uint16_t)payload[0] | ((uint16_t)payload[1] << 8);
        // store/use bearing here
        sendBinary(RSP_ACK, (uint8_t*)"B", 1);
      } else {
        sendBinary(RSP_NAK, (uint8_t*)"LEN", 3);
      }
      break;

    case CMD_RIGHT_PIVOT:
      sendBinary(RSP_ACK, (uint8_t*)"RP", 2);
      break;

    case CMD_LEFT_PIVOT:
      sendBinary(RSP_ACK, (uint8_t*)"LP", 2);
      break;

    case CMD_WAYPOINT_DATA:
      // Expecting your 22-byte payload from ARC
      if (len == 22) {
        // Parse here as needed
        sendBinary(RSP_ACK, (uint8_t*)"WP", 2);
      } else {
        sendBinary(RSP_NAK, (uint8_t*)"WP_LEN", 6);
      }
      break;

    default:
      sendBinary(RSP_NAK, (uint8_t*)"BAD_CMD", 7);
      break;
  }
}

void processIncoming() {
  static uint8_t state = 0;
  static uint8_t cmd = 0;
  static uint8_t len = 0;
  static uint8_t idx = 0;
  static uint8_t payload[64];
  static uint8_t cs = 0;

  while (LINK_SERIAL.available()) {
    uint8_t b = (uint8_t)LINK_SERIAL.read();

    if (state == 0) {
      // Detect text line vs binary header
      if (b == UART_HEADER) {
        state = 10;
        cs = 0;
      } else if (b == '\n') {
        if (rxLine.length() > 0) {
          handleTextCommand(rxLine);
          rxLine = "";
        }
      } else if (b != '\r') {
        rxLine += (char)b;
      }
      continue;
    }

    if (state == 10) {
      cmd = b;
      cs = cmd;
      state = 11;
      continue;
    }

    if (state == 11) {
      len = b;
      cs ^= len;
      idx = 0;
      state = (len == 0) ? 13 : 12;
      continue;
    }

    if (state == 12) {
      if (idx < sizeof(payload)) {
        payload[idx++] = b;
      }
      cs ^= b;
      if (idx >= len) state = 13;
      continue;
    }

    if (state == 13) {
      if (b == cs) {
        handleBinaryPacket(cmd, payload, len);
      } else {
        sendNak("CS");
      }
      state = 0;
      cmd = 0;
      len = 0;
      idx = 0;
      cs = 0;
      continue;
    }
  }
}

void setup() {
  LINK_SERIAL.begin(BAUD);
  delay(500);
  sendReady();
}

void loop() {
  processIncoming();

  if (running) {
    irValue = analogRead(A0);
    // servo / sensor logic here
  }
}

3) Mega Firmware Sketch

This is the matching Mega side.

You said the Mega is wired as:

• Mega TX3 pin14 -> EZB RX0
• Mega RX3 pin15 -> EZB TX0

That means the Mega should use Serial3.

Mega firmware

#include <Arduino.h>

static const uint32_t BAUD = 115200;
static const uint8_t UART_HEADER = 0xA5;

#define LINK_SERIAL Serial3

String rxLine = "";
bool systemReady = false;
bool motorsEnabled = false;
uint32_t stepCount = 0;

// -------------------------
// Binary command IDs
// -------------------------
enum CommandId : uint8_t {
  CMD_STOP_MOTORS     = 0x01,
  CMD_ACTUAL_BEARING  = 0x03,
  CMD_RIGHT_PIVOT     = 0x04,
  CMD_LEFT_PIVOT      = 0x05,
  CMD_WAYPOINT_DATA   = 0x06
};

// -------------------------
// Binary response IDs
// -------------------------
enum ResponseId : uint8_t {
  RSP_NAV_ACK         = 0x10,
  RSP_TOTAL_STEPS     = 0x11,
  RSP_DESIRED_BEARING = 0x12,
  RSP_OBS_SCAN_REQ    = 0x13,
  RSP_READY           = 0x14,
  RSP_STATUS          = 0x15,
  RSP_ACK             = 0x16,
  RSP_NAK             = 0x17
};

void sendText(const String& s) {
  LINK_SERIAL.println(s);
}

void sendReady() {
  sendText("READY,Mega,v3");
}

void sendStatus() {
  String s = "STATUS,";
  s += (motorsEnabled ? "RUNNING" : "READY");
  s += ",STEPS=";
  s += String(stepCount);
  sendText(s);
}

void sendAck(const String& msg) {
  sendText("ACK," + msg);
}

void sendNak(const String& reason) {
  sendText("NAK," + reason);
}

uint8_t checksumPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  uint8_t sum = (cmd ^ len);
  for (uint8_t i = 0; i < len; i++) {
    sum ^= payload[i];
  }
  return sum;
}

void sendBinary(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  LINK_SERIAL.write(UART_HEADER);
  LINK_SERIAL.write(cmd);
  LINK_SERIAL.write(len);
  for (uint8_t i = 0; i < len; i++) {
    LINK_SERIAL.write(payload[i]);
  }
  LINK_SERIAL.write(checksumPacket(cmd, payload, len));
}

void stopAllMotors() {
  motorsEnabled = false;
  // add real stepper stop logic here
}

void startAllMotors() {
  motorsEnabled = true;
  // add real stepper enable/start logic here
}

void handleTextCommand(String line) {
  line.trim();
  line.toUpperCase();

  if (line == "HELLO") {
    systemReady = true;
    sendReady();
    return;
  }

  if (line == "STATUS?") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    sendStatus();
    return;
  }

  if (line == "START") {
    if (!systemReady) {
      sendNak("NOT_READY");
      return;
    }
    startAllMotors();
    sendAck("START");
    return;
  }

  if (line == "STOP") {
    stopAllMotors();
    sendAck("STOP");
    return;
  }

  sendNak("UNKNOWN_CMD");
}

void handleBinaryPacket(uint8_t cmd, uint8_t* payload, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopAllMotors();
      sendBinary(RSP_ACK, (uint8_t*)"S", 1);
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        uint16_t bearing100 = (uint16_t)payload[0] | ((uint16_t)payload[1] << 8);
        // use bearing100
        sendBinary(RSP_ACK, (uint8_t*)"B", 1);
      } else {
        sendBinary(RSP_NAK, (uint8_t*)"LEN", 3);
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2) {
        uint16_t steps = (uint16_t)payload[0] | ((uint16_t)payload[1] << 8);
        // right pivot logic
        sendBinary(RSP_ACK, (uint8_t*)"RP", 2);
      } else {
        sendBinary(RSP_NAK, (uint8_t*)"LEN", 3);
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2) {
        uint16_t steps = (uint16_t)payload[0] | ((uint16_t)payload[1] << 8);
        // left pivot logic
        sendBinary(RSP_ACK, (uint8_t*)"LP", 2);
      } else {
        sendBinary(RSP_NAK, (uint8_t*)"LEN", 3);
      }
      break;

    case CMD_WAYPOINT_DATA:
      if (len == 22) {
        // parse waypoint payload here
        sendBinary(RSP_ACK, (uint8_t*)"WP", 2);
      } else {
        sendBinary(RSP_NAK, (uint8_t*)"WP_LEN", 6);
      }
      break;

    default:
      sendBinary(RSP_NAK, (uint8_t*)"BAD_CMD", 7);
      break;
  }
}

void processIncoming() {
  static uint8_t state = 0;
  static uint8_t cmd = 0;
  static uint8_t len = 0;
  static uint8_t idx = 0;
  static uint8_t payload[64];
  static uint8_t cs = 0;

  while (LINK_SERIAL.available()) {
    uint8_t b = (uint8_t)LINK_SERIAL.read();

    if (state == 0) {
      if (b == UART_HEADER) {
        state = 10;
        cs = 0;
      } else if (b == '\n') {
        if (rxLine.length() > 0) {
          handleTextCommand(rxLine);
          rxLine = "";
        }
      } else if (b != '\r') {
        rxLine += (char)b;
      }
      continue;
    }

    if (state == 10) {
      cmd = b;
      cs = cmd;
      state = 11;
      continue;
    }

    if (state == 11) {
      len = b;
      cs ^= len;
      idx = 0;
      state = (len == 0) ? 13 : 12;
      continue;
    }

    if (state == 12) {
      if (idx < sizeof(payload)) {
        payload[idx++] = b;
      }
      cs ^= b;
      if (idx >= len) state = 13;
      continue;
    }

    if (state == 13) {
      if (b == cs) {
        handleBinaryPacket(cmd, payload, len);
      } else {
        sendNak("CS");
      }
      state = 0;
      cmd = 0;
      len = 0;
      idx = 0;
      cs = 0;
      continue;
    }
  }
}

void setup() {
  LINK_SERIAL.begin(BAUD);
  delay(500);
  sendReady();
}

void loop() {
  processIncoming();

  if (motorsEnabled) {
    stepCount++;
    // actual stepper sequencing goes here
  }
}

4) Message Table

Text handshake messages

Binary runtime packets

Binary responses

5) Step-by-Step Manager Logic

Teensy manager flow

1. ARC opens UART2 at 115200.
2. ARC sends HELLO.
3. Teensy replies READY,Teensy,v3.
4. ARC sends STATUS?.
5. Teensy replies STATUS,....
6. ARC marks Teensy ready.
7. ARC sends binary runtime packets as needed.
8. Teensy handles each binary command and returns ACK or NAK.
9. ARC periodically sends STATUS? to confirm health.

Mega manager flow

1. ARC opens UART0 at 115200.
2. ARC sends HELLO.
3. Mega replies READY,Mega,v3.
4. ARC sends STATUS?.
5. Mega replies STATUS,....
6. ARC marks Mega ready.
7. ARC sends binary movement / waypoint packets.
8. Mega executes command and returns ACK or NAK.
9. ARC periodically polls status and watches for responses.

6) What ARC Must Handle vs What Firmware Must Handle

ARC JavaScript should:

• Initiate HELLO
• Retry if not ready
• Request STATUS?
• Send START / STOP
• Build and send binary packets
• Parse binary responses
• Update ARC global variables like $teensyReady, $megaReady, $navAckReceived, etc.

Firmware should:

• Detect HELLO
• Reply with READY,...
• Respond to STATUS?
• Accept START and STOP
• Parse binary packets
• Execute the command
• Return ACK / NAK

7) Important Wiring Note

You provided:

• Teensy 4.1 TX8 pin35 -> EZBv4 RX2 pin19
• Teensy 4.1 RX8 pin34 -> EZBv4 TX2 pin18
• Mega TX3 pin14 -> EZBv4 RX0
• Mega RX3 pin15 -> EZBv4 TX0

That is perfect for:

• Teensy = Serial8
• Mega = Serial3

So the firmware examples above are aligned with your wiring.

8) Recommended ARC Project Structure

In ARC, I would suggest:

• Script skill: UART_CommonHelper
• Script skill: TEENSY_UART_MGRv3
• Script skill: MEGA_UART_MGRv3

Each manager can call the same helper functions and only define:

• UART index
• device name
• command-specific payload handlers

This keeps both scripts almost identical, exactly as you wanted.

9) One small ARC tip

If you are editing these manager scripts in ARC, the built-in AI Assistant can help refactor the repeated logic into a clean shared helper file very quickly:

https://synthiam.com/Products/AI-Assistant

If you want, I can do the next pass and turn this into:

1. A full ARC JavaScript implementation for both managers using one shared helper style
2. A complete packet parser for all of your existing CMD_* and RSP_* IDs
3. A Teensy firmware parser that fully decodes your 22-byte waypoint payload
4. A Mega firmware implementation that maps the binary commands into your stepper functions

If you’d like, I can write the exact v3 ARC manager scripts next so they’re ready to paste into ARC.

@Athena. Thank you. Can you do the next pass and turn this into the following?

1. A full ARC JavaScript implementation for both managers using one shared helper style
2. A complete packet parser for all of your existing CMD_* and RSP_* IDs
3. A Teensy firmware parser that fully decodes your 22-byte waypoint payload
4. A Mega firmware implementation that maps the binary commands into your stepper functions

Can you please write the exact v3 ARC manager scripts next so they’re ready to paste into ARC.

I have included my existing sketch that is running on the Mega firmware. I think it may be overly complicated, it does a poor job of smoothly correcting the motors. Could you please review it and make recommendations? Can you consider how to implement the existing commands with your new Mega firmware sketch?

Thank you. Jack

Existing Mega sketch

#include <AccelStepper.h>#include <MultiStepper.h>
long pathTotalDistance = 0;long totalStepsTaken = 0;long adjacentRemainingDistance = 0;long pathEndpointDistance = 0;long oppositeDistance = 0;float desiredCourse;float lastDesiredCourse = 200;//long actualCourse;float actualCourse;float diffCourse = 0;float lastDiff = 100;int lastIncomingByte;//int targetLeftSpeed = leftSpeed;//int targetRightSpeed = leftSpeed;AccelStepper LeftFrontWheel(1, 3, 6);AccelStepper LeftBackWheel(1, 2, 5);AccelStepper RightFrontWheel(1, 12, 13);AccelStepper RightBackWheel(1, 4, 7);int run = 0;int wheelSpeed = 1600;int leftSpeed = 0;int rightSpeed = 0;int incomingByte;int initRun = 1;int currentLeftSpeed = 0;int currentRightSpeed = 0;int step = 1;  // Adjust step size for smoother or faster ramp-upint targetRightSpeed = 0;int targetLeftSpeed = 0;

void setup() {
  resetAll();  Serial3.begin(9600);  // Communication with EZ-B  Serial.begin(9600);   // Debugging via Serial Monitor  LeftFrontWheel.setMaxSpeed(wheelSpeed * 2);  LeftBackWheel.setMaxSpeed(wheelSpeed * 2);  RightFrontWheel.setMaxSpeed(wheelSpeed * 2);  RightBackWheel.setMaxSpeed(wheelSpeed * 2);}

void loop() {
Main:
  if (run == 0) {  //State of RUN when robot has yet to begin movement towards the reflector.
    // 1 header + (3 * 4 bytes for the 32-bit values) + 2 one-byte values    if (Serial3.available() >= 1) {      incomingByte = Serial3.read();      delay(100);      if (incomingByte == 'A') {  // Confirm header byte        Serial.print("incomingByte:  ");        Serial.println('A');
        // Read 4 bytes for pathTotalDistance        uint8_t b0 = Serial3.read();        uint8_t b1 = Serial3.read();        uint8_t b2 = Serial3.read();        uint8_t b3 = Serial3.read();        // Reconstruct the 32-bit integer (little-endian)        pathTotalDistance = ((long)b3 << 24) | ((long)b2 << 16) | ((long)b1 << 8) | b0;        Serial.print("pathTotalDistance: ");        Serial.println(pathTotalDistance);
        // Read 4 bytes for pathEndpointDistance        b0 = Serial3.read();        b1 = Serial3.read();        b2 = Serial3.read();        b3 = Serial3.read();        pathEndpointDistance = ((long)b3 << 24) | ((long)b2 << 16) | ((long)b1 << 8) | b0;        Serial.print("pathEndpointDistance: ");        Serial.println(pathEndpointDistance);
        // Read 4 bytes for oppositeDistance        b0 = Serial3.read();        b1 = Serial3.read();        b2 = Serial3.read();        b3 = Serial3.read();        oppositeDistance = ((long)b3 << 24) | ((long)b2 << 16) | ((long)b1 << 8) | b0;        Serial.print("oppositeDistance: ");        Serial.println(oppositeDistance);
        // Read one byte for actualCourse and one for desiredCourse        actualCourse = Serial3.read();   // one byte value        desiredCourse = Serial3.read();  // one byte value        Serial.print("actualCourse: ");        Serial.println(actualCourse);        Serial.print("desiredCourse: ");        Serial.println(desiredCourse);
        // flush the Serial3 buffer        while (Serial3.available()) {          Serial3.read();          delay(10);        }        LeftFrontWheel.setCurrentPosition(0);  //Setup for "while"loop to demonstrate trackRight        RightFrontWheel.setCurrentPosition(0);        run = 1;        Serial3.write('S');  //  Send commands to EZB4 to alert scripts of successful data transfer        Serial.print("Sent:   ");        Serial.println('S');        goto Main;
      } else if (incomingByte == 'N') {  // Stop Pivot        stopMotors();        delay(10);        Serial.print("N:  ");        Serial.println('N');        goto Main;
      } else if (incomingByte == 'L') {  // Pivot Left.        Serial.print("L :  ");        Serial.println('L');        rotateLeft();        while (RightFrontWheel.currentPosition() <= 6000) {          LeftFrontWheel.runSpeed();          LeftBackWheel.runSpeed();          RightFrontWheel.runSpeed();          RightBackWheel.runSpeed();          if (Serial3.available() > 0) {            int incomingByte = Serial3.read();  // Read the incoming byte            if (incomingByte == 'x') {              Serial.print("x0 :   ");              Serial.println('x');              stopMotors();              goto Main;            }          }        }      } else if (incomingByte == 'R') {  // Pivot Right.        Serial.print("R :  ");        Serial.println('R');        // lastIncomingByte = 'R';        rotateRight();        while (LeftFrontWheel.currentPosition() <= 6000) {          LeftFrontWheel.runSpeed();          LeftBackWheel.runSpeed();          RightFrontWheel.runSpeed();          RightBackWheel.runSpeed();          if (Serial3.available() > 0) {            int incomingByte = Serial3.read();  // Read the incoming byte            if (incomingByte == 'x') {              Serial.print("x0 :   ");              Serial.println('x');              stopMotors();              goto Main;            }          }        }      }    }  }
  //State of RUN when robot begins movement towards the reflector.  while (run == 1) {
    while ((LeftFrontWheel.currentPosition() + RightFrontWheel.currentPosition()) <= pathEndpointDistance) {
      if (Serial3.available() > 0) {  // Checking for incoming STOP command due to obstruction detected or inability to track reflector.        // Read the incoming byte        int incomingByte = Serial3.read();        // Check if the incoming byte is 'x'        if (incomingByte == 'x') {          Serial.print("x :   ");          Serial.println('x');          stopMotors();          goto Main;        } else {          actualCourse = incomingByte;  // Constant update of actualCourse for error state calculations.          Serial.print("A/C:   ");          Serial.println(actualCourse);        }      }      if (desiredCourse != 90) {  // Required calculation when reflector is offset from robot path.        toa();        if (desiredCourse != lastDesiredCourse) {          Serial3.write("desiredCourse");              lastDesiredCourse = desiredCourse;
         // Serial.print("Sent DC:  ");          //Serial.println(desiredCourse);        }      }
      diffCourse = actualCourse - desiredCourse;  // Determines if error state exists between actualCourse and desiredCourse.
      if (diffCourse != lastDiff) {        lastDiff = diffCourse;        leftSpeed = map(diffCourse, -1, 1, (wheelSpeed + 75), (wheelSpeed - 75));        rightSpeed = map(diffCourse, -1, 1, (wheelSpeed - 75), (wheelSpeed + 75));   //-275        leftSpeed = constrain(leftSpeed, (wheelSpeed - 400), (wheelSpeed + 400));    //Constrain the value of leftSpeed        rightSpeed = constrain(rightSpeed, (wheelSpeed - 400), (wheelSpeed + 400));  //Constrain the value of righttSpeed
        if (initRun == 1) {          rampUpMotors();        }      }
      LeftFrontWheel.setSpeed(leftSpeed);      LeftBackWheel.setSpeed(rightSpeed);      RightFrontWheel.setSpeed(rightSpeed);      RightBackWheel.setSpeed(leftSpeed);
      //Serial.print("leftSpeed  ");      // Serial.println(leftSpeed);      // Serial.print("wheelSpeed:  ");      // Serial.println(wheelSpeed);      // Serial.print("rightSpeed:  ");      // Serial.println(rightSpeed);
      LeftFrontWheel.runSpeed();      LeftBackWheel.runSpeed();      RightFrontWheel.runSpeed();      RightBackWheel.runSpeed();    }
    stopMotors();    goto Main;  }  delay(100);}

void rotateRight() {
  LeftFrontWheel.setCurrentPosition(0);  LeftFrontWheel.setSpeed(wheelSpeed * .15);  LeftBackWheel.setSpeed(wheelSpeed * .15);  RightFrontWheel.setSpeed(-wheelSpeed * .15);  RightBackWheel.setSpeed(-wheelSpeed * .15);}
void rotateLeft() {
  RightFrontWheel.setCurrentPosition(0);  LeftFrontWheel.setSpeed(-wheelSpeed * .15);  LeftBackWheel.setSpeed(-wheelSpeed * .15);  RightFrontWheel.setSpeed(wheelSpeed * .15);  RightBackWheel.setSpeed(wheelSpeed * .15);}

void stopMotors() {  //  For motor stop
  LeftFrontWheel.setSpeed(0);  LeftBackWheel.setSpeed(0);  RightFrontWheel.setSpeed(0);  RightBackWheel.setSpeed(0);
  LeftFrontWheel.runSpeed();  LeftBackWheel.runSpeed();  RightFrontWheel.runSpeed();  RightBackWheel.runSpeed();
  if (run == 1) {
    totalStepsTaken = (LeftFrontWheel.currentPosition() + RightFrontWheel.currentPosition());    int byte0 = totalStepsTaken & 0xFF;    int byte1 = (totalStepsTaken >> 8) & 0xFF;    int byte2 = (totalStepsTaken >> 16) & 0xFF;    int byte3 = (totalStepsTaken >> 24) & 0xFF;
    Serial3.write('T');  //Header for Total Steps Taken
    // Send the four bytes one by one.    Serial3.write(byte0);    Serial3.write(byte1);    Serial3.write(byte2);    Serial3.write(byte3);

    Serial.print("Sent   ");    Serial.println('T');    Serial.print("Left Steps:  ");    Serial.println(LeftFrontWheel.currentPosition());    Serial.print("Right Steps:  ");    Serial.println(RightFrontWheel.currentPosition());    Serial.print("Sent TST:  ");    Serial.println(totalStepsTaken);    Serial.println("  ");  }  bufferFlush();  resetAll();}

void resetAll() {  //  Reset some variable

  diffCourse = 0;  lastDiff = 100;  run = 0;  LeftFrontWheel.setCurrentPosition(0);  //Setup for "while"loop to demonstrate trackRight  RightFrontWheel.setCurrentPosition(0);  initRun = 1;  currentLeftSpeed = 0;  currentRightSpeed = 0;  targetRightSpeed = 0;  targetLeftSpeed = 0;}
void toa() {  //  Trigonometry calculation of a right triangle.  Used for offset reflector  adjacentRemainingDistance = (pathTotalDistance - (LeftFrontWheel.currentPosition() + RightFrontWheel.currentPosition()));  // Use atan2 to calculate the angle correctly  desiredCourse = (atan2(oppositeDistance, adjacentRemainingDistance) * 57.32) - 90;  // a positive angle is required, use the absolute value  desiredCourse = abs(desiredCourse);  desiredCourse = round(desiredCourse);  // Serial.print( "DC:  ");  // Serial.println( desiredCourse);
  // Serial.print("DC:  ");  // Serial.println(desiredCourse);  //Serial.print("   ");}
void bufferFlush() {  while (Serial3.available()) {    Serial3.read();  // Discard any unwanted data    delay(10);  }}void rampUpMotors() {  // Motor movement command at initial start of tracking RUN towards reflector.
  targetLeftSpeed = leftSpeed;  targetRightSpeed = rightSpeed;
  while (currentLeftSpeed < targetLeftSpeed || currentRightSpeed < targetRightSpeed) {    if (currentLeftSpeed < targetLeftSpeed) {      currentLeftSpeed += step;    }    if (currentLeftSpeed > targetLeftSpeed) {      currentLeftSpeed = targetLeftSpeed;      //initRun = 0;    }    if (currentRightSpeed < targetRightSpeed) {      currentRightSpeed += step;    }    if (currentRightSpeed > targetRightSpeed) {      currentRightSpeed = targetRightSpeed;    }
    //Serial.print("currentLeftSpeed  ");    //Serial.println(currentLeftSpeed);    //Serial.print("currentRightSpeed  ");    //Serial.println(currentRightSpeed);
    LeftFrontWheel.setSpeed(currentLeftSpeed);    LeftBackWheel.setSpeed(currentRightSpeed);    RightFrontWheel.setSpeed(currentRightSpeed);    RightBackWheel.setSpeed(currentLeftSpeed);
    LeftFrontWheel.runSpeed();    LeftBackWheel.runSpeed();    RightFrontWheel.runSpeed();    RightBackWheel.runSpeed();  }  leftSpeed = currentLeftSpeed;  rightSpeed = currentRightSpeed;  initRun = 0;
  Serial.print("currentLeftSpeed  ");  Serial.println(currentLeftSpeed);  Serial.print("currentRightSpeed  ");  Serial.println(currentRightSpeed);}

Ultima

Absolutely - and thank you for the existing Mega sketch. It helps a lot because I can see why the current behavior feels jerky: the sketch is heavily blocking, uses goto, mixes transport parsing with motion logic, and ramps speed in a way that only updates when the error changes rather than continuously smoothing toward target speeds.

Below is the v3 rewrite direction I recommend:

• ARC does handshake + sends binary commands
• Mega firmware becomes a non-blocking state machine
• Teensy firmware becomes a clean parser/worker
• Shared ARC helper handles both UART devices
• Your existing command ideas are preserved, but the runtime is made much smoother

What I recommend changing

Problems in the current Mega sketch

Your current code has a few things that make smoothing difficult:

1. Blocking while loops

   • The sketch often sits inside a loop until movement ends.
   • That prevents frequent small updates, which are needed for smooth correction.

2. goto Main and nested state blocks

   • Hard to maintain and easy to break.
   • Makes reconnect/recovery logic difficult.

3. UART protocol is ad hoc

   • A, R, x, T are not self-describing enough for long-term growth.

4. Speed correction is not truly continuous

   • The current algorithm updates motor speeds only when diffCourse changes.
   • For smooth control, you want a continuous control loop with:
     • target speed
     • current speed
     • gradual convergence

5. Motor control and packet parsing are tightly coupled

   • Better to parse into a command struct, then let the motion loop consume it.

v3 architecture I recommend

ARC side

• Send HELLO
• Wait for READY
• Send STATUS?
• Parse STATUS
• Send binary command packets
• Receive binary responses

Mega side

• Parse text handshake commands
• Parse binary payloads for:
  • waypoint data
  • pivot commands
  • stop command
  • actual bearing updates
• Run a non-blocking motor update loop
• Apply smoothing / ramping every cycle

Teensy side

• Same handshake pattern
• Binary commands for servo / IR sensor / status
• Optional periodic telemetry

1) Full ARC JavaScript implementation

I’m going to give you two scripts:

• TEENSY_UART_MGRv3
• MEGA_UART_MGRv3

They share the same helper pattern and are ready to paste into ARC.

Shared helper pattern

ARC does not have true file imports like a full desktop environment, so the easiest approach is to keep a shared section at the top of both scripts and only change:

• UART_INDEX
• DEVICE_NAME
• the device-specific packet handlers

TEENSY_UART_MGRv3

var UART_INDEX = 2; // EZB UART2 -> Teensy
var UART_BAUD = 115200;
var DEVICE_NAME = "Teensy";

var UART_HEADER = 0xA5;

var deviceReady = false;
var lastHelloMs = 0;
var lastStatusMs = 0;
var running = true;

// -------------------------
// Command IDs
// -------------------------
var CMD_STOP_MOTORS = 0x01;
var CMD_ACTUAL_BEARING = 0x03;
var CMD_RIGHT_PIVOT = 0x04;
var CMD_LEFT_PIVOT = 0x05;
var CMD_WAYPOINT_DATA = 0x06;

// -------------------------
// Response IDs
// -------------------------
var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;
var RSP_READY = 0x14;
var RSP_STATUS = 0x15;
var RSP_ACK = 0x16;
var RSP_NAK = 0x17;

// -------------------------
// Helpers
// -------------------------
function nowMs() {
  return (new Date()).getTime();
}

function safeInt(v, def) {
  var n = parseInt(v);
  if (isNaN(n)) return def;
  return n;
}

function clamp(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

function u16ToBytes(v) {
  v = v & 0xFFFF;
  return [v & 0xFF, (v >> 8) & 0xFF];
}

function u32ToBytes(v) {
  v = v >>> 0;
  return [
    v & 0xFF,
    (v >> 8) & 0xFF,
    (v >> 16) & 0xFF,
    (v >> 24) & 0xFF
  ];
}

function bytesToU16LE(b0, b1) {
  return ((b0 & 0xFF) | ((b1 & 0xFF) << 8)) >>> 0;
}

function bytesToU32LE(b0, b1, b2, b3) {
  return ((b0 & 0xFF) |
    ((b1 & 0xFF) << 8) |
    ((b2 & 0xFF) << 16) |
    ((b3 & 0xFF) << 24)) >>> 0;
}

function sendText(line) {
  print("TX [" + DEVICE_NAME + "]: " + line);
  UART.hardwareUartWriteString(UART_INDEX, line + "\n");
}

function requestHello() {
  sendText("HELLO");
  lastHelloMs = nowMs();
}

function requestStatus() {
  sendText("STATUS?");
  lastStatusMs = nowMs();
}

function sendStart() {
  sendText("START");
}

function sendStop() {
  sendText("STOP");
}

function checksumPacket(cmd, payload) {
  var sum = (cmd ^ payload.length) & 0xFF;
  for (var i = 0; i < payload.length; i++) {
    sum ^= (payload[i] & 0xFF);
  }
  return sum & 0xFF;
}

function sendPacket(cmd, payload) {
  var packet = [UART_HEADER, cmd & 0xFF, payload.length & 0xFF];
  for (var i = 0; i < payload.length; i++) packet.push(payload[i] & 0xFF);
  packet.push(checksumPacket(cmd, payload));
  UART.hardwareUartWrite(UART_INDEX, packet);
  print("TX BIN [" + DEVICE_NAME + "] cmd=0x" + toHex2(cmd) + " len=" + payload.length);
}

function toHex2(v) {
  var hex = "0123456789ABCDEF";
  return hex.charAt((v >> 4) & 0x0F) + hex.charAt(v & 0x0F);
}

// -------------------------
// Device-specific commands
// -------------------------
function sendStopMotors() {
  sendPacket(CMD_STOP_MOTORS, []);
}

function sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
}

function sendRightPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_RIGHT_PIVOT, u16ToBytes(steps));
}

function sendLeftPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_LEFT_PIVOT, u16ToBytes(steps));
}

function sendWaypointData() {
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;
  var desiredBearing100 = clamp(safeInt(getVar("$desiredBearing100", 9000), 9000), 0, 18000);
  var actualBearing100 = clamp(safeInt(getVar("$actualBearing100", 9000), 9000), 0, 18000);
  var scanIntervalSteps = safeInt(getVar("$scanIntervalSteps", 3056), 3056) >>> 0;

  setVar("$desiredBearing100", desiredBearing100);
  setVar("$actualBearing100", actualBearing100);

  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload = payload.concat(u16ToBytes(actualBearing100));
  payload.push(trackDir);
  payload = payload.concat(u32ToBytes(scanIntervalSteps));

  if (payload.length !== 22) {
    print("Waypoint payload error, len=" + payload.length);
    return;
  }

  sendPacket(CMD_WAYPOINT_DATA, payload);
}

// -------------------------
// Response handling
// -------------------------
function handleTextLine(line) {
  var parts = line.split(",");
  var head = parts[0];

  if (head == "READY") {
    deviceReady = true;
    print(DEVICE_NAME + " READY: " + line);
    setVar("$" + DEVICE_NAME.toLowerCase() + "Ready", 1);
    return;
  }

  if (head == "ACK") {
    print(DEVICE_NAME + " ACK: " + line);
    return;
  }

  if (head == "NAK") {
    print(DEVICE_NAME + " NAK: " + line);
    return;
  }

  if (head == "STATUS") {
    print(DEVICE_NAME + " STATUS: " + line);
    return;
  }

  print(DEVICE_NAME + " TEXT: " + line);
}

function handleBinaryPacket(cmd, payload) {
  if (cmd == RSP_READY) {
    deviceReady = true;
    print(DEVICE_NAME + " BIN READY");
    setVar("$" + DEVICE_NAME.toLowerCase() + "Ready", 1);
    return;
  }

  if (cmd == RSP_ACK) {
    print(DEVICE_NAME + " BIN ACK");
    return;
  }

  if (cmd == RSP_NAK) {
    print(DEVICE_NAME + " BIN NAK");
    return;
  }

  if (cmd == RSP_NAV_ACK) {
    setVar("$navAckReceived", 1);
    print(DEVICE_NAME + " NAV ACK");
    return;
  }

  if (cmd == RSP_TOTAL_STEPS) {
    if (payload.length == 4) {
      var steps = bytesToU32LE(payload[0], payload[1], payload[2], payload[3]);
      setVar("$totalStepsTaken", steps);
      setVar("$legComplete", 1);
      print(DEVICE_NAME + " TOTAL STEPS = " + steps);
    }
    return;
  }

  if (cmd == RSP_DESIRED_BEARING) {
    if (payload.length == 2) {
      var bearing100 = bytesToU16LE(payload[0], payload[1]);
      setVar("$desiredBearing100", bearing100);
      print(DEVICE_NAME + " DESIRED BEARING x100 = " + bearing100);
    }
    return;
  }

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    if (getVar("$obsScanActive", 0) == 1) {
      print(DEVICE_NAME + " obstacle scan ignored - already active");
      return;
    }
    setVar("$obsScanRequested", 1);
    setVar("$obsScanActive", 1);
    print(DEVICE_NAME + " obstacle scan requested");
    return;
  }

  print(DEVICE_NAME + " unknown binary cmd=" + cmd + " len=" + payload.length);
}

// -------------------------
// Binary parser
// -------------------------
var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

function processIncomingBytes() {
  var available = UART.hardwareUartAvailable(UART_INDEX);

  while (available > 0) {
    var b = UART.hardwareUartRead(UART_INDEX, 1)[0] & 0xFF;
    available--;

    if (rxState == 0) {
      if (b == UART_HEADER) {
        rxState = 1;
      } else {
        // Could be text line start; ignore until a text parser is added at line granularity
      }
      continue;
    }

    if (rxState == 1) {
      rxCmd = b;
      rxChecksum = (rxCmd ^ 0) & 0xFF;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b & 0xFF;
      rxChecksum ^= rxLen;
      rxPayload = [];
      rxIndex = 0;
      rxState = (rxLen == 0) ? 4 : 3;
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b & 0xFF);
      rxChecksum ^= (b & 0xFF);
      rxIndex++;
      if (rxIndex >= rxLen) rxState = 4;
      continue;
    }

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        handleBinaryPacket(rxCmd, rxPayload);
      } else {
        print(DEVICE_NAME + " checksum error expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

      rxState = 0;
      rxCmd = 0;
      rxLen = 0;
      rxIndex = 0;
      rxPayload = [];
      rxChecksum = 0;
      continue;
    }
  }
}

function maintainHandshake() {
  var now = nowMs();

  if (!deviceReady && (now - lastHelloMs) > 2000) {
    requestHello();
  }

  if (deviceReady && (now - lastStatusMs) > 5000) {
    requestStatus();
  }
}

// -------------------------
// Main
// -------------------------
print("Starting " + DEVICE_NAME + " manager");
UART.initHardwareUart(UART_INDEX, UART_BAUD);
requestHello();
sleep(200);
requestStatus();

while (true) {
  processIncomingBytes();
  maintainHandshake();

  if (getVar("$sendWaypointNow", 0) == 1 && getVar("$waypointSent", 0) == 0) {
    sendWaypointData();
    setVar("$sendWaypointNow", 0);
    setVar("$waypointSent", 1);
  }

  if (getVar("$sendStopNow", 0) == 1 && getVar("$stopSent", 0) == 0) {
    sendStopMotors();
    setVar("$sendStopNow", 0);
    setVar("$stopSent", 1);
  }

  if (getVar("$sendActualBearingNow", 0) == 1) {
    sendActualBearing();
    setVar("$sendActualBearingNow", 0);
  }

  if (getVar("$sendRightPivotNow", 0) == 1) {
    sendRightPivot();
    setVar("$sendRightPivotNow", 0);
  }

  if (getVar("$sendLeftPivotNow", 0) == 1) {
    sendLeftPivot();
    setVar("$sendLeftPivotNow", 0);
  }

  sleep(20);
}

MEGA_UART_MGRv3

var UART_INDEX = 0; // EZB UART0 -> Mega
var UART_BAUD = 115200;
var DEVICE_NAME = "Mega";

var UART_HEADER = 0xA5;

var deviceReady = false;
var lastHelloMs = 0;
var lastStatusMs = 0;
var running = true;

// -------------------------
// Command IDs
// -------------------------
var CMD_STOP_MOTORS = 0x01;
var CMD_ACTUAL_BEARING = 0x03;
var CMD_RIGHT_PIVOT = 0x04;
var CMD_LEFT_PIVOT = 0x05;
var CMD_WAYPOINT_DATA = 0x06;

// -------------------------
// Response IDs
// -------------------------
var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;
var RSP_READY = 0x14;
var RSP_STATUS = 0x15;
var RSP_ACK = 0x16;
var RSP_NAK = 0x17;

// -------------------------
// Helpers
// -------------------------
function nowMs() {
  return (new Date()).getTime();
}

function safeInt(v, def) {
  var n = parseInt(v);
  if (isNaN(n)) return def;
  return n;
}

function clamp(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

function u16ToBytes(v) {
  v = v & 0xFFFF;
  return [v & 0xFF, (v >> 8) & 0xFF];
}

function u32ToBytes(v) {
  v = v >>> 0;
  return [
    v & 0xFF,
    (v >> 8) & 0xFF,
    (v >> 16) & 0xFF,
    (v >> 24) & 0xFF
  ];
}

function bytesToU16LE(b0, b1) {
  return ((b0 & 0xFF) | ((b1 & 0xFF) << 8)) >>> 0;
}

function bytesToU32LE(b0, b1, b2, b3) {
  return ((b0 & 0xFF) |
    ((b1 & 0xFF) << 8) |
    ((b2 & 0xFF) << 16) |
    ((b3 & 0xFF) << 24)) >>> 0;
}

function sendText(line) {
  print("TX [" + DEVICE_NAME + "]: " + line);
  UART.hardwareUartWriteString(UART_INDEX, line + "\n");
}

function requestHello() {
  sendText("HELLO");
  lastHelloMs = nowMs();
}

function requestStatus() {
  sendText("STATUS?");
  lastStatusMs = nowMs();
}

function sendStart() {
  sendText("START");
}

function sendStop() {
  sendText("STOP");
}

function checksumPacket(cmd, payload) {
  var sum = (cmd ^ payload.length) & 0xFF;
  for (var i = 0; i < payload.length; i++) {
    sum ^= (payload[i] & 0xFF);
  }
  return sum & 0xFF;
}

function sendPacket(cmd, payload) {
  var packet = [UART_HEADER, cmd & 0xFF, payload.length & 0xFF];
  for (var i = 0; i < payload.length; i++) packet.push(payload[i] & 0xFF);
  packet.push(checksumPacket(cmd, payload));
  UART.hardwareUartWrite(UART_INDEX, packet);
  print("TX BIN [" + DEVICE_NAME + "] cmd=0x" + toHex2(cmd) + " len=" + payload.length);
}

function toHex2(v) {
  var hex = "0123456789ABCDEF";
  return hex.charAt((v >> 4) & 0x0F) + hex.charAt(v & 0x0F);
}

// -------------------------
// Device-specific commands
// -------------------------
function sendStopMotors() {
  sendPacket(CMD_STOP_MOTORS, []);
}

function sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
}

function sendRightPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_RIGHT_PIVOT, u16ToBytes(steps));
}

function sendLeftPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_LEFT_PIVOT, u16ToBytes(steps));
}

function sendWaypointData() {
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;
  var desiredBearing100 = clamp(safeInt(getVar("$desiredBearing100", 9000), 9000), 0, 18000);
  var actualBearing100 = clamp(safeInt(getVar("$actualBearing100", 9000), 9000), 0, 18000);
  var scanIntervalSteps = safeInt(getVar("$scanIntervalSteps", 3056), 3056) >>> 0;

  setVar("$desiredBearing100", desiredBearing100);
  setVar("$actualBearing100", actualBearing100);

  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload = payload.concat(u16ToBytes(actualBearing100));
  payload.push(trackDir);
  payload = payload.concat(u32ToBytes(scanIntervalSteps));

  if (payload.length !== 22) {
    print("Waypoint payload error, len=" + payload.length);
    return;
  }

  sendPacket(CMD_WAYPOINT_DATA, payload);
}

// -------------------------
// Response handling
// -------------------------
function handleTextLine(line) {
  var parts = line.split(",");
  var head = parts[0];

  if (head == "READY") {
    deviceReady = true;
    print(DEVICE_NAME + " READY: " + line);
    setVar("$megaReady", 1);
    return;
  }

  if (head == "ACK") {
    print(DEVICE_NAME + " ACK: " + line);
    return;
  }

  if (head == "NAK") {
    print(DEVICE_NAME + " NAK: " + line);
    return;
  }

  if (head == "STATUS") {
    print(DEVICE_NAME + " STATUS: " + line);
    return;
  }

  print(DEVICE_NAME + " TEXT: " + line);
}

function handleBinaryPacket(cmd, payload) {
  if (cmd == RSP_READY) {
    deviceReady = true;
    print(DEVICE_NAME + " BIN READY");
    setVar("$megaReady", 1);
    return;
  }

  if (cmd == RSP_ACK) {
    print(DEVICE_NAME + " BIN ACK");
    return;
  }

  if (cmd == RSP_NAK) {
    print(DEVICE_NAME + " BIN NAK");
    return;
  }

  if (cmd == RSP_NAV_ACK) {
    setVar("$navAckReceived", 1);
    print(DEVICE_NAME + " NAV ACK");
    return;
  }

  if (cmd == RSP_TOTAL_STEPS) {
    if (payload.length == 4) {
      var steps = bytesToU32LE(payload[0], payload[1], payload[2], payload[3]);
      setVar("$totalStepsTaken", steps);
      setVar("$legComplete", 1);
      print(DEVICE_NAME + " TOTAL STEPS = " + steps);
    }
    return;
  }

  if (cmd == RSP_DESIRED_BEARING) {
    if (payload.length == 2) {
      var bearing100 = bytesToU16LE(payload[0], payload[1]);
      setVar("$desiredBearing100", bearing100);
      print(DEVICE_NAME + " DESIRED BEARING x100 = " + bearing100);
    }
    return;
  }

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    setVar("$obsScanRequested", 1);
    setVar("$obsScanActive", 1);
    print(DEVICE_NAME + " obstacle scan requested");
    return;
  }

  print(DEVICE_NAME + " unknown binary cmd=" + cmd + " len=" + payload.length);
}

// -------------------------
// Binary parser
// -------------------------
var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

function processIncomingBytes() {
  var available = UART.hardwareUartAvailable(UART_INDEX);

  while (available > 0) {
    var b = UART.hardwareUartRead(UART_INDEX, 1)[0] & 0xFF;
    available--;

    if (rxState == 0) {
      if (b == UART_HEADER) {
        rxState = 1;
      } else {
        // ignore
      }
      continue;
    }

    if (rxState == 1) {
      rxCmd = b;
      rxChecksum = (rxCmd ^ 0) & 0xFF;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b & 0xFF;
      rxChecksum ^= rxLen;
      rxPayload = [];
      rxIndex = 0;
      rxState = (rxLen == 0) ? 4 : 3;
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b & 0xFF);
      rxChecksum ^= (b & 0xFF);
      rxIndex++;
      if (rxIndex >= rxLen) rxState = 4;
      continue;
    }

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        handleBinaryPacket(rxCmd, rxPayload);
      } else {
        print(DEVICE_NAME + " checksum error expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

      rxState = 0;
      rxCmd = 0;
      rxLen = 0;
      rxIndex = 0;
      rxPayload = [];
      rxChecksum = 0;
      continue;
    }
  }
}

function maintainHandshake() {
  var now = nowMs();

  if (!deviceReady && (now - lastHelloMs) > 2000) {
    requestHello();
  }

  if (deviceReady && (now - lastStatusMs) > 5000) {
    requestStatus();
  }
}

// -------------------------
// Main
// -------------------------
print("Starting " + DEVICE_NAME + " manager");
UART.initHardwareUart(UART_INDEX, UART_BAUD);
requestHello();
sleep(200);
requestStatus();

while (true) {
  processIncomingBytes();
  maintainHandshake();

  if (getVar("$sendWaypointNow", 0) == 1 && getVar("$waypointSent", 0) == 0) {
    sendWaypointData();
    setVar("$sendWaypointNow", 0);
    setVar("$waypointSent", 1);
  }

  if (getVar("$sendStopNow", 0) == 1 && getVar("$stopSent", 0) == 0) {
    sendStopMotors();
    setVar("$sendStopNow", 0);
    setVar("$stopSent", 1);
  }

  if (getVar("$sendActualBearingNow", 0) == 1) {
    sendActualBearing();
    setVar("$sendActualBearingNow", 0);
  }

  if (getVar("$sendRightPivotNow", 0) == 1) {
    sendRightPivot();
    setVar("$sendRightPivotNow", 0);
  }

  if (getVar("$sendLeftPivotNow", 0) == 1) {
    sendLeftPivot();
    setVar("$sendLeftPivotNow", 0);
  }

  sleep(20);
}

2) Complete packet parser for CMD_* and RSP_* IDs

Here is the recommended handling map for v3.

Commands ARC sends to firmware

Responses firmware sends to ARC

3) Teensy firmware parser for the 22-byte waypoint payload

Below is the payload layout I recommend for CMD_WAYPOINT_DATA:

22-byte payload layout

If you want, I can also write the exact Teensyduino decode code for this packet in the next message, but the parsing logic is:

• read 22 bytes
• decode each field
• store into a structured command object
• use that object to drive servo/IR logic

4) Mega firmware implementation recommendations

What I recommend for the Mega motion loop

Your current motor logic should be converted to:

State variables

• systemReady
• motorsEnabled
• currentLeftSpeed
• currentRightSpeed
• targetLeftSpeed
• targetRightSpeed
• pathTotalDistance
• totalStepsTaken
• desiredCourse
• actualCourse
• diffCourse

Loop structure

1. Process incoming UART bytes
2. If a full packet arrived, update target parameters
3. Compute desired speed corrections
4. Smoothly ramp current speeds toward targets
5. Call runSpeed() on all four steppers
6. Repeat

That removes the blocking loops.

Recommended smoothing strategy

Instead of:

• jump to new speed when diff changes

Use:

• currentLeftSpeed += constrain(targetLeftSpeed - currentLeftSpeed, -step, step)
• currentRightSpeed += constrain(targetRightSpeed - currentRightSpeed, -step, step)

This creates smooth motor correction.

5) How to implement your existing commands with the new Mega firmware

Here is how your existing behaviors map into v3.

Existing behavior: path tracking / reflectance correction

Replace the current while(run == 1) behavior with:

• receive CMD_WAYPOINT_DATA
• decode:
  • path distance
  • target steps
  • offset distance
  • bearing alignment
  • direction flags
• compute:
  • desiredCourse
  • diffCourse
  • target wheel speeds
• ramp smoothly

Existing behavior: pivot right

Map to:

• CMD_RIGHT_PIVOT
• decode step count
• set pivot state
• drive wheels in opposite directions until step count met
• send RSP_ACK and optionally RSP_TOTAL_STEPS

Existing behavior: pivot left

Map to:

• CMD_LEFT_PIVOT
• same structure as pivot right

Existing behavior: stop

Map to:

• CMD_STOP_MOTORS
• immediately zero targets
• set motorsEnabled = false
• send total steps in a response

Existing behavior: actual bearing updates

Map to:

• CMD_ACTUAL_BEARING
• update actualCourse
• recompute error

6) Concrete recommendation for the new Mega firmware structure

I strongly recommend splitting the Mega sketch into these functions:

• processIncoming()
• handleTextCommand()
• handleBinaryPacket()
• applyWaypointData()
• updateMotionTargets()
• rampMotorSpeeds()
• runMotors()
• stopMotors()

This will make the firmware much easier to debug than the current monolithic loop().

7) ARC setup note

In ARC, add each manager as its own Robot Skill:

• TEENSY_UART_MGRv3
• MEGA_UART_MGRv3

Then configure them through the skill’s kebab menu:

1. Open the skill
2. Click the three-line / kebab menu
3. Set any startup options or script variables
4. Save

8) Important follow-up

I can do the next step and give you the full Mega C++ firmware rewrite in the same v3 protocol style, including:

• exact binary packet decode
• stepper control functions
• non-blocking state machine
• smoothing/ramp logic
• total step reporting
• pivot handling
• waypoint decode

If you want, I can also convert your current sketch directly into:

1. A cleaned-up Mega v3 firmware sketch
2. A cleaned-up Teensy v3 firmware sketch
3. A finished ARC script pair ready to paste into ARC

If you’d like, I’ll do the Mega firmware rewrite first since that’s the part most in need of restructuring.

@Athena. Thank you very much for your help. I want to be very clear what the function of each microcontroller is responsible for. Looking at the code for Mega mgr and Teensy mgr, I'm not sure we share the same vision for this rover. I'm probably just misunderstanding,

Teensy firmware:

1. Sweeps the servo left and right.
2. Pulses IR for reflector detection and totals IR for obstacle detection.
3. Computes actual bearing and sends new actual bearings to EZB firmware.
4. Sends STOP to EZB firmware if obstacle totals exceed limits or servo runaway occurs without reflector detection.

That's it. Nothing more. Simply produces an actual bearing and/or a STOP command.

The Mega does everything else. Mega firmware:

RECEIVES LEG INFO (from EZB firmware)

1. setVar("$pathTotalDistance", pathTotalDistance);
2. setVar("$combinedStepsDesired", combinedStepsDesired);
3. setVar("$oppositeDistance", oppositeDistance);
4. setVar("$bearingAlignment", bearingAlignment);
5. setVar("$desiredBearing", desiredBearing);
6. setVar("$actualBearing", actualBearing);
7. setVar("$trackDir", trackDir);

RUN:

1. Controls stepper motors, counts pulses for distance measurements, pivots left and right.
2. Computes desired bearing (it changes during reflector offset tracking). Periodically sends updated desired bearings to EZB firmware for monitoring purposes.
3. Receives actual bearing updates from EZB firmware. Computes the difference between desired bearing and actual bearing for motor control.

The EZB starts and monitors the Teensy and Mega. EZB firmware:

1. UART scripts which liaison between Teensy and Mega
2. 'Robot Command' script starts and stops all scripts running on EZB. Commands UART start of Teensy and Mega.
3. 'Leg Info' script creates global variables that are sent to Mega.
4. 'Monitor' script prints necessary UART info received from Teensy and Mega. I love the simple, logical, organized approach you are taking, especially with the motor control. Please continue with what should come next.

Can you do the next step and give you the full Mega C++ firmware rewrite in the same v3 protocol style? including:

• exact binary packet decode
• stepper control functions
• non-blocking state machine
• smoothing/ramp logic
• total step reporting
• pivot handling
• waypoint decode

If you want, I can also convert your current sketch directly into:

1. A cleaned-up Mega v3 firmware sketch
2. A cleaned-up Teensy v3 firmware sketch
3. A finished ARC script pair ready to paste into ARC

If you’d like, I’ll do the Mega firmware rewrite first since that’s the part most in need of restructuring.

Thank you very much. Jack

Absolutely - that clarifies the architecture very well, and I agree with your division of responsibilities.

Summary of the roles

Teensy

Only does:

• Servo sweep
• IR pulse / reflector sensing
• Obstacle counting
• Actual bearing calculation
• Sends:
  • actual bearing updates
  • STOP when obstacle / runaway conditions occur

Mega

Does everything else:

• Receives leg info from ARC/EZB
• Drives and counts the stepper motors
• Handles pivots
• Computes desired bearing
• Periodically reports desired bearing back to ARC/EZB
• Receives actual bearing updates from ARC/EZB
• Uses desired vs actual bearing difference for motor correction

EZB / ARC

Acts as the orchestrator:

• UART bridge / monitor
• Starts/stops both controllers
• Publishes leg variables
• Watches Teensy and Mega messages

That is a clean and very good split.

Next step: Mega v3 firmware rewrite

Below is a non-blocking Mega firmware rewrite in the same v3 protocol style, designed around your description:

• Binary packets for leg info and runtime commands
• Text handshake for startup
• Non-blocking state machine
• Smooth ramping
• Pivot handling
• Step counting
• Desired-bearing updates
• Actual-bearing input
• STOP handling

I’m going to make one practical assumption:

• The Mega communicates to ARC/EZB over Serial3
• Your steppers are driven with AccelStepper
• You want the Mega to be able to:
  • receive a leg info packet
  • receive actual bearing updates
  • receive stop
  • send desired bearing updates
  • send step totals
  • run its motion loop without blocking

1) Mega v3 firmware sketch

#include <AccelStepper.h>

// ======================================================
// UART / Protocol
// ======================================================
static const uint32_t BAUD = 115200;
static const uint8_t UART_HEADER = 0xA5;

// Commands from ARC/EZB
enum CommandId : uint8_t {
  CMD_STOP_MOTORS       = 0x01,
  CMD_ACTUAL_BEARING    = 0x03,
  CMD_RIGHT_PIVOT       = 0x04,
  CMD_LEFT_PIVOT        = 0x05,
  CMD_WAYPOINT_DATA     = 0x06
};

// Responses back to ARC/EZB
enum ResponseId : uint8_t {
  RSP_NAV_ACK           = 0x10,
  RSP_TOTAL_STEPS       = 0x11,
  RSP_DESIRED_BEARING   = 0x12,
  RSP_OBS_SCAN_REQUEST  = 0x13,
  RSP_READY             = 0x14,
  RSP_STATUS            = 0x15,
  RSP_ACK               = 0x16,
  RSP_NAK               = 0x17
};

#define LINK_SERIAL Serial3

// ======================================================
// Stepper pins
// ======================================================
AccelStepper LeftFrontWheel(AccelStepper::DRIVER, 3, 6);
AccelStepper LeftBackWheel(AccelStepper::DRIVER, 2, 5);
AccelStepper RightFrontWheel(AccelStepper::DRIVER, 12, 13);
AccelStepper RightBackWheel(AccelStepper::DRIVER, 4, 7);

// ======================================================
// Motion state
// ======================================================
enum MotionMode {
  MODE_IDLE = 0,
  MODE_TRACKING = 1,
  MODE_PIVOT_RIGHT = 2,
  MODE_PIVOT_LEFT = 3
};

MotionMode mode = MODE_IDLE;

bool systemReady = false;
bool motorsEnabled = false;

// ======================================================
// Leg / navigation data
// ======================================================
uint32_t pathTotalDistance = 0;
uint32_t combinedStepsDesired = 0;
uint32_t oppositeDistance = 0;
uint8_t bearingAlignment = 0;
uint16_t desiredBearing100 = 9000;   // 90.00 degrees default
uint16_t actualBearing100 = 9000;    // 90.00 degrees default
uint8_t trackDir = 0;
uint32_t scanIntervalSteps = 3056;

// ======================================================
// Runtime counters
// ======================================================
uint32_t totalStepsTaken = 0;
uint32_t lastReportedSteps = 0;
uint32_t lastDesiredBearingReportMs = 0;
uint32_t lastMotionUpdateMs = 0;
uint32_t lastBearingUpdateMs = 0;

// ======================================================
// Speed control
// ======================================================
int wheelSpeedBase = 1600;
int leftTargetSpeed = 0;
int rightTargetSpeed = 0;
int leftCurrentSpeed = 0;
int rightCurrentSpeed = 0;
int speedRampStep = 25;  // smaller = smoother

// ======================================================
// Tracking control
// ======================================================
float desiredCourse = 90.0f;
float actualCourse = 90.0f;
float diffCourse = 0.0f;
float lastDiffCourse = 9999.0f;
int lastBearingAlignment = -1;

bool pivotActive = false;
bool stopRequested = false;

// ======================================================
// UART RX parser
// Packet = [0xA5][cmd][len][payload...][checksum]
// checksum = cmd ^ len ^ payload bytes
// ======================================================
uint8_t rxState = 0;
uint8_t rxCmd = 0;
uint8_t rxLen = 0;
uint8_t rxIndex = 0;
uint8_t rxChecksum = 0;
uint8_t rxPayload[64];

// ======================================================
// Helpers
// ======================================================
void sendText(const String& s) {
  LINK_SERIAL.println(s);
}

void sendReady() {
  sendText("READY,Mega,v3");
}

void sendAck(const String& msg) {
  sendText("ACK," + msg);
}

void sendNak(const String& msg) {
  sendText("NAK," + msg);
}

uint8_t checksumPacket(uint8_t cmd, uint8_t len, const uint8_t* payload) {
  uint8_t cs = cmd ^ len;
  for (uint8_t i = 0; i < len; i++) {
    cs ^= payload[i];
  }
  return cs;
}

void sendBinaryPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  uint8_t cs = checksumPacket(cmd, len, payload);
  LINK_SERIAL.write(UART_HEADER);
  LINK_SERIAL.write(cmd);
  LINK_SERIAL.write(len);
  for (uint8_t i = 0; i < len; i++) {
    LINK_SERIAL.write(payload[i]);
  }
  LINK_SERIAL.write(cs);
}

void sendBinaryPacket0(uint8_t cmd) {
  sendBinaryPacket(cmd, nullptr, 0);
}

uint16_t bytesToU16LE(uint8_t b0, uint8_t b1) {
  return (uint16_t)b0 | ((uint16_t)b1 << 8);
}

uint32_t bytesToU32LE(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3) {
  return (uint32_t)b0 |
         ((uint32_t)b1 << 8) |
         ((uint32_t)b2 << 16) |
         ((uint32_t)b3 << 24);
}

void u16ToBytesLE(uint16_t value, uint8_t* out) {
  out[0] = value & 0xFF;
  out[1] = (value >> 8) & 0xFF;
}

void u32ToBytesLE(uint32_t value, uint8_t* out) {
  out[0] = value & 0xFF;
  out[1] = (value >> 8) & 0xFF;
  out[2] = (value >> 16) & 0xFF;
  out[3] = (value >> 24) & 0xFF;
}

// ======================================================
// Motor helpers
// ======================================================
void stopMotors() {
  leftTargetSpeed = 0;
  rightTargetSpeed = 0;
  motorsEnabled = false;
  mode = MODE_IDLE;
  pivotActive = false;
}

void applyMotorSpeeds() {
  LeftFrontWheel.setSpeed(leftCurrentSpeed);
  LeftBackWheel.setSpeed(rightCurrentSpeed);
  RightFrontWheel.setSpeed(rightCurrentSpeed);
  RightBackWheel.setSpeed(leftCurrentSpeed);

  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();
}

void rampTowardTargets() {
  if (leftCurrentSpeed < leftTargetSpeed) {
    leftCurrentSpeed += speedRampStep;
    if (leftCurrentSpeed > leftTargetSpeed) leftCurrentSpeed = leftTargetSpeed;
  } else if (leftCurrentSpeed > leftTargetSpeed) {
    leftCurrentSpeed -= speedRampStep;
    if (leftCurrentSpeed < leftTargetSpeed) leftCurrentSpeed = leftTargetSpeed;
  }

  if (rightCurrentSpeed < rightTargetSpeed) {
    rightCurrentSpeed += speedRampStep;
    if (rightCurrentSpeed > rightTargetSpeed) rightCurrentSpeed = rightTargetSpeed;
  } else if (rightCurrentSpeed > rightTargetSpeed) {
    rightCurrentSpeed -= speedRampStep;
    if (rightCurrentSpeed < rightTargetSpeed) rightCurrentSpeed = rightTargetSpeed;
  }
}

void setTrackingSpeedsFromDiff(float diff) {
  // Tune these values for your rover
  int correction = (int)(diff * 18.0f);

  leftTargetSpeed = wheelSpeedBase + correction;
  rightTargetSpeed = wheelSpeedBase - correction;

  if (leftTargetSpeed > wheelSpeedBase + 400) leftTargetSpeed = wheelSpeedBase + 400;
  if (leftTargetSpeed < wheelSpeedBase - 400) leftTargetSpeed = wheelSpeedBase - 400;

  if (rightTargetSpeed > wheelSpeedBase + 400) rightTargetSpeed = wheelSpeedBase + 400;
  if (rightTargetSpeed < wheelSpeedBase - 400) rightTargetSpeed = wheelSpeedBase - 400;
}

void reportTotalStepsIfChanged() {
  uint32_t currentSteps = (uint32_t)(abs(LeftFrontWheel.currentPosition()) + abs(RightFrontWheel.currentPosition()));
  totalStepsTaken = currentSteps;

  if (abs((int32_t)(totalStepsTaken - lastReportedSteps)) >= 25) {
    uint8_t payload[4];
    u32ToBytesLE(totalStepsTaken, payload);
    sendBinaryPacket(RSP_TOTAL_STEPS, payload, 4);
    lastReportedSteps = totalStepsTaken;
  }
}

void reportDesiredBearingIfChanged() {
  if (millis() - lastDesiredBearingReportMs < 250) return;

  uint8_t payload[2];
  u16ToBytesLE(desiredBearing100, payload);
  sendBinaryPacket(RSP_DESIRED_BEARING, payload, 2);
  lastDesiredBearingReportMs = millis();
}

void computeDesiredBearing() {
  // If reflector tracking changes bearing over a leg,
  // keep the math here and update desiredBearing100.
  //
  // This is where you can recreate your old toa() behavior
  // in a non-blocking way.
  //
  // For now, use the commanded desiredBearing100 as the live setpoint.
  desiredCourse = desiredBearing100 / 100.0f;
}

void updateMotionControl() {
  if (!motorsEnabled || stopRequested) {
    stopMotors();
    applyMotorSpeeds();
    return;
  }

  if (mode == MODE_PIVOT_RIGHT || mode == MODE_PIVOT_LEFT) {
    // Pivot speed handling
    rampTowardTargets();
    applyMotorSpeeds();
    return;
  }

  if (mode == MODE_TRACKING) {
    actualCourse = actualBearing100 / 100.0f;
    diffCourse = actualCourse - desiredCourse;

    if (diffCourse != lastDiffCourse) {
      setTrackingSpeedsFromDiff(diffCourse);
      lastDiffCourse = diffCourse;
    }

    rampTowardTargets();
    applyMotorSpeeds();
    reportDesiredBearingIfChanged();
    reportTotalStepsIfChanged();
    return;
  }

  applyMotorSpeeds();
}

// ======================================================
// Command handlers
// ======================================================
void handleWaypointPacket(const uint8_t* p, uint8_t len) {
  if (len != 22) {
    sendNak("WAYPOINT_LEN");
    return;
  }

  pathTotalDistance     = bytesToU32LE(p[0], p[1], p[2], p[3]);
  combinedStepsDesired  = bytesToU32LE(p[4], p[5], p[6], p[7]);
  oppositeDistance      = bytesToU32LE(p[8], p[9], p[10], p[11]);
  bearingAlignment      = p[12];
  desiredBearing100     = bytesToU16LE(p[13], p[14]);
  actualBearing100     = bytesToU16LE(p[15], p[16]);
  trackDir              = p[17];
  scanIntervalSteps     = bytesToU32LE(p[18], p[19], p[20], p[21]);

  desiredCourse = desiredBearing100 / 100.0f;
  actualCourse = actualBearing100 / 100.0f;

  motorsEnabled = true;
  mode = MODE_TRACKING;
  stopRequested = false;
  pivotActive = false;

  sendAck("WAYPOINT");
  sendBinaryPacket0(RSP_NAV_ACK);
}

void handleActualBearingPacket(const uint8_t* p, uint8_t len) {
  if (len != 2) {
    sendNak("BEARING_LEN");
    return;
  }

  actualBearing100 = bytesToU16LE(p[0], p[1]);
  actualCourse = actualBearing100 / 100.0f;

  sendAck("BEARING");
}

void handlePivotRightPacket(const uint8_t* p, uint8_t len) {
  if (len != 2) {
    sendNak("PIVOT_LEN");
    return;
  }

  uint16_t pivotSteps = bytesToU16LE(p[0], p[1]);

  LeftFrontWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);

  mode = MODE_PIVOT_RIGHT;
  motorsEnabled = true;
  pivotActive = true;
  stopRequested = false;

  leftTargetSpeed = wheelSpeedBase / 6;
  rightTargetSpeed = -(wheelSpeedBase / 6);

  sendAck("PIVOT_RIGHT");

  // You can optionally store pivotSteps and stop when reached
  // using a non-blocking check in the loop.
  (void)pivotSteps;
}

void handlePivotLeftPacket(const uint8_t* p, uint8_t len) {
  if (len != 2) {
    sendNak("PIVOT_LEN");
    return;
  }

  uint16_t pivotSteps = bytesToU16LE(p[0], p[1]);

  LeftFrontWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);

  mode = MODE_PIVOT_LEFT;
  motorsEnabled = true;
  pivotActive = true;
  stopRequested = false;

  leftTargetSpeed = -(wheelSpeedBase / 6);
  rightTargetSpeed = wheelSpeedBase / 6;

  sendAck("PIVOT_LEFT");

  (void)pivotSteps;
}

void handleStopPacket() {
  stopRequested = true;
  motorsEnabled = false;
  stopMotors();

  uint8_t payload[4];
  u32ToBytesLE(totalStepsTaken, payload);
  sendBinaryPacket(RSP_TOTAL_STEPS, payload, 4);
  sendAck("STOP");
}

void handleBinaryPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      handleStopPacket();
      break;

    case CMD_ACTUAL_BEARING:
      handleActualBearingPacket(payload, len);
      break;

    case CMD_RIGHT_PIVOT:
      handlePivotRightPacket(payload, len);
      break;

    case CMD_LEFT_PIVOT:
      handlePivotLeftPacket(payload, len);
      break;

    case CMD_WAYPOINT_DATA:
      handleWaypointPacket(payload, len);
      break;

    default:
      sendNak("UNKNOWN_CMD");
      break;
  }
}

// ======================================================
// Incoming parser
// ======================================================
void processIncoming() {
  while (LINK_SERIAL.available() > 0) {
    uint8_t b = (uint8_t)LINK_SERIAL.read();

    if (rxState == 0) {
      if (b == UART_HEADER) {
        rxState = 1;
      }
      continue;
    }

    if (rxState == 1) {
      rxCmd = b;
      rxChecksum = rxCmd;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b;
      rxChecksum ^= rxLen;
      rxIndex = 0;
      if (rxLen > sizeof(rxPayload)) {
        rxState = 0;
        sendNak("LEN_OVF");
      } else {
        rxState = (rxLen == 0) ? 4 : 3;
      }
      continue;
    }

    if (rxState == 3) {
      rxPayload[rxIndex++] = b;
      rxChecksum ^= b;
      if (rxIndex >= rxLen) {
        rxState = 4;
      }
      continue;
    }

    if (rxState == 4) {
      if (b == rxChecksum) {
        handleBinaryPacket(rxCmd, rxPayload, rxLen);
      } else {
        sendNak("CS");
      }

      rxState = 0;
      rxCmd = 0;
      rxLen = 0;
      rxIndex = 0;
      rxChecksum = 0;
      continue;
    }
  }
}

// ======================================================
// Text handshake / startup
// ======================================================
String textLine = "";

void processTextLine(const String& line) {
  String s = line;
  s.trim();
  s.toUpperCase();

  if (s == "HELLO") {
    systemReady = true;
    sendReady();
    return;
  }

  if (s == "STATUS?") {
    String msg = "STATUS,";
    msg += (motorsEnabled ? "RUNNING" : "IDLE");
    msg += ",MODE=";
    msg += String((int)mode);
    msg += ",STEPS=";
    msg += String(totalStepsTaken);
    sendText(msg);
    return;
  }

  if (s == "START") {
    motorsEnabled = true;
    mode = MODE_TRACKING;
    sendAck("START");
    return;
  }

  if (s == "STOP") {
    handleStopPacket();
    return;
  }

  sendNak("UNKNOWN_TEXT");
}

void processTextSerial() {
  while (LINK_SERIAL.available() > 0) {
    char c = (char)LINK_SERIAL.read();

    if (c == '\n') {
      if (textLine.length() > 0) {
        processTextLine(textLine);
        textLine = "";
      }
    } else if (c != '\r') {
      textLine += c;
    }
  }
}

// ======================================================
// Setup / loop
// ======================================================
void setup() {
  LINK_SERIAL.begin(BAUD);
  Serial.begin(BAUD);

  LeftFrontWheel.setMaxSpeed(wheelSpeedBase * 2);
  LeftBackWheel.setMaxSpeed(wheelSpeedBase * 2);
  RightFrontWheel.setMaxSpeed(wheelSpeedBase * 2);
  RightBackWheel.setMaxSpeed(wheelSpeedBase * 2);

  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(0);

  sendReady();
}

void loop() {
  processIncoming();
  processTextSerial();

  computeDesiredBearing();
  updateMotionControl();

  if (pivotActive) {
    uint32_t steps = (uint32_t)abs(LeftFrontWheel.currentPosition()) + (uint32_t)abs(RightFrontWheel.currentPosition());
    if (steps >= combinedStepsDesired && combinedStepsDesired > 0) {
      stopRequested = true;
      motorsEnabled = false;
      pivotActive = false;
      stopMotors();
      reportTotalStepsIfChanged();
    }
  }

  delay(1);
}

2) What this Mega rewrite changes

Exact binary packet decode

The Mega now cleanly decodes:

• CMD_STOP_MOTORS
• CMD_ACTUAL_BEARING
• CMD_RIGHT_PIVOT
• CMD_LEFT_PIVOT
• CMD_WAYPOINT_DATA

Non-blocking state machine

No more nested infinite loops or goto Main.

Smooth ramping

Speed changes are applied gradually in rampTowardTargets().

Pivot handling

Pivot commands are handled as a motion mode.

Step reporting

Total steps are reported back as binary response packets.

Desired bearing reporting

Desired bearing is reported periodically.

3) Recommendations for improving the motion quality

Your old sketch is trying to do three things at once:

• movement control
• UART parsing
• path math

That is why it feels unstable.

Better approach

Use this separation:

A. UART parser

Only handles:

• packet framing
• packet validation
• command decode

B. Motion state

Tracks:

• idle
• tracking
• pivoting
• stopped

C. Control loop

Runs every loop iteration:

• calculate bearing diff
• set target speeds
• ramp speed toward targets
• call runSpeed()

That gives you a much smoother robot.

4) How your existing commands map into this v3 firmware

Existing leg info from EZB

These variables become the 22-byte CMD_WAYPOINT_DATA payload:

1. $pathTotalDistance
2. $combinedStepsDesired
3. $oppositeDistance
4. $bearingAlignment
5. $desiredBearing
6. $actualBearing
7. $trackDir

Mega behavior

• Receives leg info
• Tracks movement
• Computes desired bearing
• Updates target wheel speeds
• Sends desired bearing updates back
• Receives actual bearing updates from EZB
• Uses the difference for correction

Teensy behavior

• Sweeps servo
• Reads IR
• Computes actual bearing
• Sends actual bearing to EZB
• Sends STOP to EZB if necessary

5) Important note about your old Mega sketch

One important thing: your current sketch appears to be mixing:

• stepper motion control
• serial command decoding
• waypoint correction math
• stop handling
• turning logic

It also uses blocking while-loops that make the robot less responsive.

If you want smooth correction, the best next improvement is to make the target speed change continuously based on bearing error, rather than only when the error changes. That’s what the new rampTowardTargets() flow is designed for.

6) What I recommend next

The best next step is for me to do two things for you:

1. Write the Teensy v3 firmware

   • servo sweep
   • IR pulse / obstacle total
   • actual bearing calculation
   • STOP transmit logic

2. Refine the ARC manager scripts

   • so the EZB can send leg info cleanly
   • and monitor both microcontrollers

If you want, I can do the Teensy v3 firmware next and make it match this Mega rewrite exactly.

Also, if you’d like to turn these scripts into ARC robot skills, remember the skill settings are accessed from the kebab menu on the top right of each skill window.

If you want, I can continue with:

• Teensy v3 firmware rewrite
• Final ARC manager scripts
• A leg-info packet builder for ARC
• A cleaner speed-correction algorithm for the Mega

Just say which one you want next.

@Athena. Thank you for MEGAv3 Firmware Sketch. Under the subtitle:

Mega behavior

• Receives leg info
• Tracks movement
• Computes desired bearing
• Updates target wheel speeds
• Sends desired bearing updates back
• Receives actual bearing updates from EZB
• Uses the difference for correction

Sends desired bearing updates back. This only needs to accomplish via UART to EZB 'Monitor script' whenever the desired bearing changes by 1 degree. EZB 'Monitor script' prints the actual bearing as it is received by the Teensy which declares a new actual bearing only when change is significant. The 'Monitor script' prints are for my evaluation of tracking accuracy. I chose 1 degree variance for desired bearing so as not to bog down UART with unnecessary tasks.

Can you please rewrite MEGAv3 Firmware Sketch?

1. to allow 1 time printing of received leg information.
2. to allow 1 time printing of desired bearing if it changes by 1 deg or more.
3. to allow 1 time printing of actual bearing when received.
4. to allow 1 time printing of STOP command sent or recieved. Can you continue with the following?

• Teensy v3 firmware rewrite

I wish to be able to run the Teensy-v3 and the Mega-v3 together to confirm their mutual compatibility. Fine tuning this pair seems appropriate after test running them on the rover together.

I have attached my EZB 'LEG' script so that you can see how I have been using it. Please feel free to update/rewrite it so that I can verify data is being sent properly via UART. Also verifying that the Teensy-v3 and Mega-v3 work together. I had been printing the desired bearing/actual bearings from a previous UART script for 'Monitoring'. Can you write a 'Monitoring' script that prints the desired bearing and actual bearing from their global variables?

Your Statement: Also, if you’d like to turn these scripts into ARC robot skills, remember the skill settings are accessed from the kebab menu on the top right of each skill window. My Question: Is there a difference in the terms 'script' and 'ARC robot skill'? My understanding of kebab menu is that this is how a script is accessed and changed. 'Saving' it closes the script. Is this correct?

Thank you. Jack

EZB LEGv1

print("LEG_v1 started");

// ---------- Helpers ----------
function safeInt(v, def) {
  var n = parseInt(v);
  if (isNaN(n)) return def;
  return n;
}

function clampInt(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

// ---------- Initial values ----------
// Read current servo position on port d17
var bearingPos = safeInt(Servo.getPosition(d17), 0);

// Use the servo position as both starting values
var actualBearing = bearingPos * 100;
var desiredBearing = bearingPos * 100;

// Example leg values
var pathTotalDistance = safeInt(14000, 0);
var combinedStepsDesired = safeInt(11000, 0);
var oppositeDistance = safeInt(300, 0);
var bearingAlignment = safeInt(0, 0); // 0 = DA, 1 = OA
var trackDir = safeInt(0, 0);         // 0 = FD, 1 = RD

// Clamp to expected ranges
pathTotalDistance = clampInt(pathTotalDistance, 0, 120000);
combinedStepsDesired = clampInt(combinedStepsDesired, 0, 84000);
oppositeDistance = clampInt(oppositeDistance, 0, 24000);
bearingAlignment = clampInt(bearingAlignment, 0, 1);
trackDir = clampInt(trackDir, 0, 1);

// Store globals for UART_v5 / other skills
setVar("$pathTotalDistance", pathTotalDistance);
setVar("$combinedStepsDesired", combinedStepsDesired);
setVar("$oppositeDistance", oppositeDistance);
setVar("$bearingAlignment", bearingAlignment);
setVar("$desiredBearing", desiredBearing);
setVar("$actualBearing", actualBearing);
setVar("$trackDir", trackDir);

// Control flags
setVar("$waypointSent", 0);
setVar("$sendWaypointNow", 1);
setVar("$sendStopNow", 0);
setVar("$sendActualBearingNow", 0);
setVar("$obsScanRequested", 0); //0
setVar("$obsScanActive", 0);
setVar("$run", 0);
setVar("$legComplete", 0);
setVar("$totalStepsTaken", 0);

print("LEG_v5 loaded:");
print("pathTotalDistance = " + pathTotalDistance);
print("combinedStepsDesired = " + combinedStepsDesired);
print("oppositeDistance = " + oppositeDistance);
print("bearingAlignment = " + bearingAlignment);
print("desiredBearing = " + desiredBearing);
print("actualBearing = " + actualBearing);
print("trackDir = " + trackDir);

print("$obsScanRequested:  " + getVar("$obsScanRequested"));