Yes - keep them separate.

That’s the better long-term choice because:

• Teensy UART2 has its own packet format and behavior
• Mega UART0 has a different binary protocol
• Separate scripts are easier to debug, test, and restart independently
• You can still share a few helper functions later if you want, but I would not merge the live task scripts into one

Recommended ARC layout

ARC robot skills

• EZ-B Connection skill

  • Connects ARC to the EZ-B v4 over Wi-Fi
  • Use connection index 0

• Script skill: Teensy UART Manager

  • Talks to the Teensy on UART2
  • Uses D18 = TX and D19 = RX
  • Baud: 115200

• Script skill: Mega UART Manager

  • Talks to the Mega on UART0
  • Uses your Serial Expansion Port path
  • Baud: whatever your Mega firmware expects

1) Final polished Teensy v4.2.1

Below is a cleaned-up version with:

• packet framing $PAYLOAD*HH
• ACK / NAK
• STATUS?
• HELLO / READY handshake
• no periodic unsolicited status
• packet handler separated out
• cleaner startup behavior

PacketHandler.h

#pragma once
#include <Arduino.h>

struct PacketResult {
  bool valid;
  String payload;
};

uint8_t simpleChecksum(const String& payload);
String toHex2(uint8_t value);
PacketResult parsePacket(const String& packet);
String buildPacket(const String& payload);

PacketHandler.cpp

#include "PacketHandler.h"

uint8_t simpleChecksum(const String& payload) {
  uint8_t sum = 0;
  for (size_t i = 0; i < payload.length(); i++) {
    sum ^= (uint8_t)payload.charAt(i);
  }
  return sum;
}

String toHex2(uint8_t value) {
  const char* hex = "0123456789ABCDEF";
  String out;
  out += hex[(value >> 4) & 0x0F];
  out += hex[value & 0x0F];
  return out;
}

PacketResult parsePacket(const String& packet) {
  PacketResult result;
  result.valid = false;
  result.payload = "";

  int star = packet.indexOf('*');
  if (star < 0) return result;
  if (packet.length() < 4) return result;
  if (packet.charAt(0) != '$') return result;
  if (star + 2 >= packet.length()) return result;

  String payload = packet.substring(1, star);
  String checksumText = packet.substring(star + 1, star + 3);

  uint8_t received = (uint8_t) strtol(checksumText.c_str(), nullptr, 16);
  uint8_t computed = simpleChecksum(payload);

  if (received != computed) return result;

  result.valid = true;
  result.payload = payload;
  return result;
}

String buildPacket(const String& payload) {
  uint8_t chk = simpleChecksum(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

IR_TRACKERv4.2.1.h

#pragma once
#include <Arduino.h>
#include <Servo.h>

enum TrackerState {
  NEG_SEARCH_INIT,
  NEG_SEARCH_FINAL,
  POS_SEARCH_INIT,
  POS_SEARCH_FINAL,
  TRACKER_STOPPED
};

static const uint8_t SERVO_PIN = 29;
static const uint8_t IR_PIN = A14;

static const int SERVO_MIN_ANGLE = 5;
static const int SERVO_MAX_ANGLE = 175;
static const int SERVO_MIN_PULSE = 500;
static const int SERVO_MAX_PULSE = 2500;

static const int DESIRED_BEARING = 90;
static const int STEP_DELTA = 1;
static const int RUNAWAY_MARGIN = 20;
static const int RESET_DELAY_MS = 20;
static const int SETTLE_DELAY_MS = 30;

static const int IR_THRESHOLD = 15;
static const int IR_SMOOTH_SAMPLES = 8;
static const int ADC_MAX_RAW = 4095;

static const long NEG_OBS_MAX = 2000;
static const long POS_OBS_MAX = 2000;

static const unsigned long EZB_BAUD = 115200;

extern servo trackingServo;

extern TrackerState state;
extern int desiredBearing;
extern int roundedBearing;
extern int actualBearing;
extern int bearing;

extern int negInit;
extern int negFinal;
extern int posInit;
extern int posFinal;

extern long negObsTot;
extern long posObsTot;

extern int bearingCount;
extern int runawayCount;
extern int negAbortLimit;
extern int posAbortLimit;

extern bool stopRequested;
extern bool startRequested;

void setupTracker();
void loopTracker();

void beginNegSweep();
void beginPosSweep();
void doNegSearchInit();
void doNegSearchFinal();
void doPosSearchInit();
void doPosSearchFinal();

void stopNow(const char* reason);
void processTrackerState();

int normalizeIRTo255(int raw);
int readIRSmooth();
int clampServoAngle(int angle);
void setServoBearing(int angle);
int readServo();

void publishBearing(int candidate);
void updateBearingFromInitEdges();
void updateBearingFromFinalEdges();
void handleRunaway();

void pollSerial();
void handleSerialCommand(const String& cmd);

void sendToEZB(const String& msg);
void sendBearingUpdate();
void sendBearingCount();
void sendStatus();
void sendAck(const String& cmd);
void sendNak(const String& reason);

IR_TRACKERv4.2.1.cpp

#include "IR_TRACKERv4.2.1.h"
#include "PacketHandler.h"

Servo trackingServo;

TrackerState state = NEG_SEARCH_INIT;
int desiredBearing = DESIRED_BEARING;
int roundedBearing = DESIRED_BEARING;
int actualBearing = DESIRED_BEARING;
int bearing = DESIRED_BEARING;

int negInit = -1;
int negFinal = -1;
int posInit = -1;
int posFinal = -1;

long negObsTot = 0;
long posObsTot = 0;

int bearingCount = 0;
int runawayCount = 0;
int negAbortLimit = 0;
int posAbortLimit = 0;

bool stopRequested = false;
bool startRequested = false;

static int irSamples[IR_SMOOTH_SAMPLES];
static int irSampleIndex = 0;
static bool irInitialized = false;

static unsigned long lastDebugMs = 0;
static int lastActualBearing = -1;
static int lastPublishedBearing = -1;
static int prevLastPublishedBearing = -1;

static String serialPacket;
static bool inPacket = false;
static unsigned long packetStartMs = 0;

static int clampAngle(int angle) {
  return constrain(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
}

int clampServoAngle(int angle) {
  return clampAngle(angle);
}

void setServoBearing(int angle) {
  bearing = clampAngle(angle);
  trackingServo.write(bearing);
}

int readServo() {
  return bearing;
}

int normalizeIRTo255(int raw) {
  raw = constrain(raw, 0, ADC_MAX_RAW);
  return map(raw, 0, ADC_MAX_RAW, 0, 255);
}

int readIRSmooth() {
  int raw = analogRead(IR_PIN);

  if (!irInitialized) {
    for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
      irSamples[i] = raw;
    }
    irInitialized = true;
  }

  irSamples[irSampleIndex] = raw;
  irSampleIndex = (irSampleIndex + 1) % IR_SMOOTH_SAMPLES;

  long total = 0;
  for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
    total += irSamples[i];
  }

  return (int)(total / IR_SMOOTH_SAMPLES);
}

void sendToEZB(const String& msg) {
  Serial8.print(buildPacket(msg));
}

void sendAck(const String& cmd) {
  sendToEZB("ACK," + cmd);
}

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

void sendBearingUpdate() {
  sendToEZB("BEARING," + String(actualBearing));
}

void sendBearingCount() {
  sendToEZB("COUNT," + String(bearingCount));
}

void sendStatus() {
  String s = "STATUS,STATE=" + String((int)state);
  s += ",BEARING=" + String(bearing);
  s += ",ACTUAL=" + String(actualBearing);
  s += ",COUNT=" + String(bearingCount);
  s += ",START=" + String(startRequested ? 1 : 0);
  s += ",STOP=" + String(stopRequested ? 1 : 0);
  sendToEZB(s);
}

void publishBearing(int candidate) {
  roundedBearing = candidate;

  if (roundedBearing == lastActualBearing) {
    if (roundedBearing != lastPublishedBearing && roundedBearing != prevLastPublishedBearing) {
      prevLastPublishedBearing = lastPublishedBearing;
      lastPublishedBearing = roundedBearing;
      actualBearing = roundedBearing;
      Serial.print("actualBearing: ");
      Serial.println(actualBearing);
      sendBearingUpdate();
    }
  } else {
    lastActualBearing = roundedBearing;
  }
}

void updateBearingFromInitEdges() {
  if (negInit >= 0 && posInit >= 0) {
    int candidate = (negInit + posInit) / 2;
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

void updateBearingFromFinalEdges() {
  if (negFinal >= 0 && posFinal >= 0) {
    int candidate = (negFinal + posFinal) / 2;
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

void stopNow(const char* reason) {
  Serial.print("STOP: ");
  Serial.println(reason);

  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  sendToEZB("STOP");
  sendBearingCount();

  stopRequested = true;
  state = TRACKER_STOPPED;
}

void beginNegSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (negObsTot > NEG_OBS_MAX) {
    Serial.print("NEG OBS total exceeded max: ");
    Serial.println(negObsTot);
    stopNow("NEG OBS total exceeded max");
    return;
  }

  if (posObsTot > POS_OBS_MAX) {
    Serial.print("POS OBS total exceeded max: ");
    Serial.println(posObsTot);
    stopNow("POS OBS total exceeded max");
    return;
  }

  negObsTot = 0;
  posObsTot = 0;
  negInit = -1;
  negFinal = -1;
  negAbortLimit = roundedBearing - RUNAWAY_MARGIN;
  state = NEG_SEARCH_INIT;
}

void beginPosSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  posInit = -1;
  posFinal = -1;
  posAbortLimit = roundedBearing + RUNAWAY_MARGIN;
  state = POS_SEARCH_INIT;
}

void handleRunaway() {
  runawayCount++;
  Serial.print("runawayCount: ");
  Serial.println(runawayCount);

  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (runawayCount >= 3) {
    stopNow("Runaway limit reached");
    return;
  }

  if (state == NEG_SEARCH_INIT) {
    beginPosSweep();
    Serial.println("POS SWEEP");
  } else {
    beginNegSweep();
    Serial.println("NEG SWEEP");
  }
}

void doNegSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negInit = current;
    updateBearingFromInitEdges();
    state = NEG_SEARCH_FINAL;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doNegSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negFinal = current;
    updateBearingFromFinalEdges();
    state = POS_SEARCH_INIT;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posInit = current;
    updateBearingFromInitEdges();
    state = POS_SEARCH_FINAL;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posFinal = current;
    updateBearingFromFinalEdges();
    state = NEG_SEARCH_INIT;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void handleSerialCommand(const String& cmd) {
  String s = cmd;
  s.trim();
  s.toUpperCase();

  if (s == "HELLO") {
    sendToEZB("READY,IR_TRACKERv4.2.1");
    sendAck("HELLO");
    return;
  }

  if (s == "START") {
    startRequested = true;
    stopRequested = false;
    sendAck("START");
    return;
  }

  if (s == "STOP") {
    stopNow("Serial STOP");
    sendAck("STOP");
    return;
  }

  if (s == "STATUS?") {
    sendStatus();
    sendAck("STATUS?");
    return;
  }

  if (s.startsWith("BEARING,")) {
    int value = s.substring(8).toInt();
    roundedBearing = clampServoAngle(value);
    setServoBearing(roundedBearing);
    sendAck("BEARING");
    return;
  }

  if (s.startsWith("COUNT,")) {
    int value = s.substring(6).toInt();
    bearingCount = value;
    sendAck("COUNT");
    return;
  }

  sendNak("UNKNOWN_CMD");
}

void pollSerial() {
  while (Serial8.available() > 0) {
    char c = (char)Serial8.read();

    if (!inPacket) {
      if (c == '$') {
        inPacket = true;
        serialPacket = "$";
        packetStartMs = millis();
      }
      continue;
    }

    if (millis() - packetStartMs > 250) {
      inPacket = false;
      serialPacket = "";
      sendNak("TIMEOUT");
      continue;
    }

    serialPacket += c;

    if (serialPacket.length() > 128) {
      inPacket = false;
      serialPacket = "";
      sendNak("OVERFLOW");
      continue;
    }

    if (c == '\n') {
      PacketResult result = parsePacket(serialPacket);
      if (result.valid) {
        handleSerialCommand(result.payload);
      } else {
        sendNak("BAD_CRC");
      }
      serialPacket = "";
      inPacket = false;
    }
  }
}

void processTrackerState() {
  if (stopRequested || !startRequested) return;

  switch (state) {
    case NEG_SEARCH_INIT: doNegSearchInit(); break;
    case NEG_SEARCH_FINAL: doNegSearchFinal(); break;
    case POS_SEARCH_INIT: doPosSearchInit(); break;
    case POS_SEARCH_FINAL: doPosSearchFinal(); break;
    case TRACKER_STOPPED:
    default:
      return;
  }

  if (millis() - lastDebugMs >= 1000) {
    lastDebugMs = millis();
    Serial.print("state=");
    Serial.print((int)state);
    Serial.print(" bearing=");
    Serial.print(bearing);
    Serial.print(" actualBearing=");
    Serial.print(actualBearing);
    Serial.print(" count=");
    Serial.print(bearingCount);
    Serial.print(" ready=");
    Serial.println(1);
  }
}

void setupTracker() {
  Serial.begin(EZB_BAUD);
  Serial8.begin(EZB_BAUD);

  delay(1000);
  analogReadResolution(12);
  pinMode(IR_PIN, INPUT);

  trackingServo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);

  actualBearing = DESIRED_BEARING;
  roundedBearing = DESIRED_BEARING;
  bearing = DESIRED_BEARING;
  setServoBearing(roundedBearing);

  startRequested = false;
  stopRequested = false;

  sendToEZB("READY,IR_TRACKERv4.2.1");
  Serial.println("IR_TRACKERv4.2.1 READY");
}

void loopTracker() {
  pollSerial();
  processTrackerState();
}

IR_TRACKERv4.2.1.ino

#include "IR_TRACKERv4.2.1.h"

void setup() {
  setupTracker();
}

void loop() {
  loopTracker();
}

2) ARC Script skill for Teensy UART2

This version uses:

• d18 = TX
• d19 = RX
• UART_INDEX = 2 for the Teensy link
• baud 115200
• ACK / NAK / STATUS handling
• startup handshake
• reconnect behavior

var UART_INDEX = 2;   // Teensy UART2
var UART_BAUD = 115200;
var UART_HEADER = 0xA5; // not used in this packet style, kept only if you later unify protocols

var running = true;
var teensyReady = false;
var lastHelloMs = 0;
var lastStatusMs = 0;
var txOpen = false;

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 = "0123456789ABCDEF";
  return hex.charAt((value >> 4) & 0x0F) + hex.charAt(value & 0x0F);
}

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

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

function requestHello() {
  sendPacket("HELLO");
  lastHelloMs = (new Date()).getTime();
}

function requestStatus() {
  sendPacket("STATUS?");
  lastStatusMs = (new Date()).getTime();
}

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

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

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

  if (head == "READY") {
    teensyReady = true;
    print("Teensy ready: " + parts[1]);
    return;
  }

  if (head == "ACK") {
    print("ACK: " + parts[1]);
    return;
  }

  if (head == "NAK") {
    print("NAK: " + parts[1]);
    return;
  }

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

  if (head == "BEARING") {
    print("BEARING: " + parts[1]);
    return;
  }

  if (head == "COUNT") {
    print("COUNT: " + parts[1]);
    return;
  }

  if (head == "STOP") {
    print("Teensy reported STOP");
    return;
  }

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

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

  if (available <= 0) return;

  var data = UART.hardwareUartReadStringAvailable(UART_INDEX);
  if (data == null || data == "") return;

  var lines = data.split("\n");
  for (var i = 0; i < lines.length; i++) {
    var line = lines[i].trim();
    if (line == "") continue;

    print("RX: " + line);

    if (line.charAt(0) != "$") continue;
    var star = line.indexOf("*");
    if (star < 0) continue;

    var payload = line.substring(1, star);
    handlePayload(payload);
  }
}

function startupHandshake() {
  teensyReady = false;
  requestHello();
  sleep(200);
  requestStatus();
  sleep(200);
  sendStart();
}

function ensureConnection() {
  if (!UART.isHardwareUartInitialized) {
    // harmless placeholder if not available in your ARC build
  }
}

print("Teensy UART2 Script starting...");

UART.initHardwareUart(UART_INDEX, UART_BAUD);
sleep(500);

startupHandshake();

while (running) {
  readIncoming();

  var now = (new Date()).getTime();

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

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

  sleep(50);
}

3) Should the Teensy and Mega scripts be merged?

No, keep them separate.

For your rover, separate scripts are better because:

• each controller has different packet rules
• one script failure won’t block the other
• troubleshooting is much easier
• you can restart one subsystem without disturbing the other

If you want, you can share a few helper ideas later, but I would still run them as two Script skills in ARC.

4) Simple project layout diagram

Laptop / PC
   |
   |  ARC
   v
EZ-B v4  <---- Wi-Fi ----  ARC EZ-B Connection skill
   |
   |-- UART0 / Serial Expansion Port --> Arduino Mega
   |
   |-- UART2 (D18 TX, D19 RX) ---------> Teensy 4.1
                                           |
                                           |-- Serial8 TX8 pin 35
                                           |-- Serial8 RX8 pin 34
                                           |-- IR sensor + servo manager

ARC skill layout

[EZ-B Connection]
        |
        +--> [Script: Mega UART Manager]
        |
        +--> [Script: Teensy UART Manager]

5) Important wiring note

Since you said:

• EZ-B D18 = TX
• EZ-B D19 = RX
• Teensy pin 34 = RX8
• Teensy pin 35 = TX8

Your wiring should be:

• EZ-B D18 TX -> Teensy RX8 / pin 34
• EZ-B D19 RX <- Teensy TX8 / pin 35
• GND to GND

And because Teensy is 3.3V logic, make sure the EZ-B UART signaling is safe for that link.

If you want, I can do one more pass and make the Mega script match the same style as the Teensy script so both UART managers in ARC look consistent.

@Athena. Can you please do one more pass and make the Mega script match the same style as the Teensy script so both UART managers in ARC look consistent?

Thank you. Jack

Absolutely - that’s a good cleanup step.

A consistent style across both ARC Script skills will make the project much easier to maintain and debug.

What I’ll do

I’ll align the Mega UART manager to the same pattern as the Teensy script:

• UART_INDEX
• UART_BAUD
• buildPacket()
• sendPacket()
• readIncoming()
• requestHello()
• requestStatus()
• simple payload dispatcher
• ACK/NAK handling
• reconnect-friendly startup loop

That way both scripts behave the same way in ARC, even though the underlying packet formats are different.

Mega UART Manager v1.0 (matching style)

This version keeps your binary packet protocol intact, but gives it the same clean structure as the Teensy manager.

var UART_INDEX = 0; // Mega UART0
var UART_BAUD = 115200;
var UART_HEADER = 0xA5;

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

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

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

// -------------------------
// Helpers
// -------------------------
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(value) {
  value = value & 0xFFFF;
  return [value & 0xFF, (value >> 8) & 0xFF];
}

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

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

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

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

function sendPacket(cmd, payload) {
  var len = payload.length & 0xFF;
  var checksum = checksumXor(cmd, len, payload);

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER, cmd, len]);
  if (payload.length > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
  }
  UART.hardwareUartWrite(UART_INDEX, [checksum]);
}

function requestHello() {
  // If your Mega firmware supports it, add a HELLO/READY handshake there too.
  // For now this is a logical startup marker in ARC.
  megaReady = false;
  lastHelloMs = (new Date()).getTime();
  print("TX: HELLO requested");
}

function requestStatus() {
  // If your Mega firmware supports STATUS? add a command ID for it.
  // For now this is a placeholder to match the Teensy manager style.
  lastStatusMs = (new Date()).getTime();
  print("TX: STATUS requested");
}

function sendStopMotors() {
  sendPacket(CMD_STOP_MOTORS, []);
  print("TX: STOP MOTORS");
}

function sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

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 length error: " + payload.length);
    return;
  }

  sendPacket(CMD_WAYPOINT_DATA, payload);
  print("TX: WAYPOINT DATA");
}

function sendPivot(cmd, name) {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  var payload = u16ToBytes(steps);
  sendPacket(cmd, payload);
  print("TX: " + name + " steps=" + steps);
}

function sendRightPivot() {
  sendPivot(CMD_RIGHT_PIVOT, "RIGHT PIVOT");
}

function sendLeftPivot() {
  sendPivot(CMD_LEFT_PIVOT, "LEFT PIVOT");
}

function requestObstacleScan() {
  if (getVar("$obsScanActive", 0) == 1) {
    print("RX: OBSTACLE SCAN REQUEST ignored (scan already active)");
    return;
  }
  setVar("$obsScanRequested", 1);
  setVar("$obsScanActive", 1);
  print("RX: OBSTACLE SCAN REQUEST -> $obsScanRequested=1, $obsScanActive=1");
}

function handleRxPacket(cmd, payload) {
  if (cmd == RSP_READY) {
    megaReady = true;
    print("RX: MEGA READY");
    return;
  }

  if (cmd == RSP_ACK) {
    print("RX: ACK");
    return;
  }

  if (cmd == RSP_NAK) {
    print("RX: NAK");
    return;
  }

  if (cmd == RSP_NAV_ACK) {
    print("RX: NAV ACK");
    setVar("$navAckReceived", 1);
    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("RX: TOTAL STEPS = " + steps);
    }
    return;
  }

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

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    requestObstacleScan();
    return;
  }

  print("RX: Unknown cmd=" + cmd + " len=" + payload.length);
}

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;
      continue;
    }

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

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

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

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

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

print("Mega UART Manager started");
UART.initHardwareUart(UART_INDEX, UART_BAUD);

requestHello();
sleep(200);
requestStatus();

while (true) {
  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);
  }

  processIncomingBytes();
  sleep(20);
}

Notes on this Mega version

A couple of important points:

• I kept your binary protocol intact.
• I matched the script structure to the Teensy script.
• I added a lightweight hello/status startup pattern.
• If your Mega firmware does not yet support HELLO, READY, or STATUS, the ARC script still works - those calls are just logical placeholders until you add the matching responses on the Mega side.

If you want, I can also rewrite the Mega firmware to support the same HELLO / READY / STATUS? / ACK / NAK pattern so both controllers behave identically.

2) How I would structure the ARC project

ARC skills

• EZ-B Connection

  • index 0
  • connects ARC to your EZ-B v4 over Wi-Fi

• Script: Mega UART Manager

  • UART0 / Serial Expansion Port
  • talks to Arduino Mega

• Script: Teensy UART Manager

  • UART2 on d18/d19
  • talks to Teensy 4.1

This is the cleanest and easiest-to-debug layout.

3) Simple project layout diagram

Laptop / PC
   |
   |  Wi-Fi
   v
ARC
   |
   |  EZ-B Connection Skill
   v
EZ-B v4
  |\
  | \-- UART0 / Serial Expansion Port ---> Arduino Mega
  |
  \---- UART2 (D18 TX, D19 RX) ----------> Teensy 4.1

Teensy wiring

EZ-B D18 (TX)  ---> Teensy RX8 pin 34
EZ-B D19 (RX)  <--- Teensy TX8 pin 35
GND            ---> GND

My recommendation

Keep the scripts separate.
That gives you:

• easier testing
• easier restarts
• cleaner debugging
• less chance of one protocol affecting the other

The style should be consistent, but the scripts should remain separate.

If you want, I can do the next pass and give you:

1. a matching Mega firmware update with HELLO / READY / STATUS? / ACK / NAK
2. a matching Teensy script that uses the same startup flow
3. a single shared ARC helper style for both scripts so they look almost identical

Thank you @Athena.