Yes - there are two likely root causes here:

1. The Mega firmware has several syntax / logic problems that would prevent reliable parsing and motor control
2. The ARC manager script is not guaranteed to be talking to the correct UART index / hardware serial pair

I’ll break this down clearly.

1) UART mapping verification

You noted:

• Mega firmware uses Serial3:

  #define LINK_SERIAL Serial3
  

• ARC manager uses:

  var UART_INDEX = 0;
  

That is only correct if the ARC-side hardware UART index 0 is actually wired to the Mega’s Serial3 pins.

On the Arduino Mega 2560

The hardware UART pins are:

• Serial = USB / UART0
• Serial1 = pins 18 (TX1) / 19 (RX1)
• Serial2 = pins 16 (TX2) / 17 (RX2)
• Serial3 = pins 14 (TX3) / 15 (RX3)

In ARC

UART.hardwareUartInit(uartIndex, baud) refers to the EZ-B’s own hardware UART index, not the Mega’s Serial numbers.

So the mapping is:

• ARC UART index 0 EZ-B hardware UART #0
• ARC UART index 1 EZ-B hardware UART #1
• ARC UART index 2 EZ-B hardware UART #2

If your ARC EZ-B hardware UART #0 is physically wired to the Mega’s Serial3, then UART_INDEX = 0 is correct.

Important physical wiring note

You must cross-connect:

• EZ-B TX Mega RX3
• EZ-B RX Mega TX3
• GND GND

And the Mega Serial3 pins are:

• RX3 = pin 15
• TX3 = pin 14

So if you’re using Serial3, the Mega should be connected to the EZ-B UART port that corresponds to your ARC hardware UART index 0.

If you want, I can help you map the exact ARC UART connector pins once you tell me which EZ-B board / UART header you’re using.

2) Why the current Mega firmware likely breaks after the first packet

Your pasted Mega_Firmware_v3 contains multiple issues that stand out immediately:

Major problems in the sketch you posted

• #include is incomplete
• bytesToU16LE() returns 0
• readU16LE() appears malformed:

  return ((uint16_t)b[0]) | (((uint16_t)b[1]) = 1)
  

  That should be << 8, not = 1
• lastReportedDeg is referenced but not declared
• updateMotionControl() is referenced but not included
• printLegInfoOnce(), printStopOnce(), printActualBearingOnce() are referenced but not included
• sendBinaryPacket() is referenced but not included
• There are several copied syntax artifacts in the code that would not compile as shown

So the pasted version is not just a little off - it looks like an incomplete or corrupted merge.

That explains why:

• the first packet may appear to work,
• but the parser or motor state machine then becomes unstable,
• and motors never run as expected.

3) Protocol verification

Your packet format is:

• Header = 0xA5
• Cmd
• Len
• Payload
• Checksum = XOR of Cmd ^ Len ^ Payload bytes

That part is consistent between the two snippets.

Good news

The ARC manager script and Mega parser both use XOR across:

• cmd
• len
• each payload byte

So the checksum logic itself is aligned.

But there is one more thing to verify

The ARC manager script sends packets only over binary UART, while your Mega sketch also tries to parse text lines from the same serial port:

• processIncoming()
• processTextSerial()

This is dangerous because:

• binary packets may contain bytes that resemble text line endings or text garbage,
• and the same serial stream is being interpreted two different ways.

For a binary protocol, I strongly recommend:

• remove text parsing from the same link, or
• use a second serial port for debug text.

4) Recommended fix: rewrite the Mega firmware cleanly

Below is a cleaner Mega_Firmware_v4 that:

• uses Serial3 for ARC link
• uses binary framing only
• properly parses packets
• sends ACK/NAK
• prints debug info to USB serial only
• includes motor placeholders in a safer structure
• avoids text parsing on the ARC link

I’ve intentionally kept this version focused and stable. You can extend the motor logic after the comms layer is verified.

Mega_Firmware_v4.cpp

#include <Arduino.h>
#include <AccelStepper.h>

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

// ARC/EZB link on Mega Serial3
#define LINK_SERIAL Serial3

// Debug to USB serial monitor
#define DBG_SERIAL Serial

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

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

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

MotionMode mode = MODE_IDLE;
bool motorsEnabled = false;
bool systemReady = false;

// Waypoint / nav data
uint32_t pathTotalDistance = 0;
uint32_t combinedStepsDesired = 0;
uint32_t oppositeDistance = 0;
uint8_t bearingAlignment = 0;
uint16_t desiredBearing100 = 9000;
uint16_t actualBearing100 = 9000;
uint8_t trackDir = 0;
uint32_t scanIntervalSteps = 3056;

// Runtime
uint32_t totalStepsTaken = 0;
uint32_t lastReportedSteps = 0;

// Motion control
int wheelSpeedBase = 1600;
int leftTargetSpeed = 0;
int rightTargetSpeed = 0;
int leftCurrentSpeed = 0;
int rightCurrentSpeed = 0;
int speedRampStep = 25;

// RX parser
uint8_t rxState = 0;
uint8_t rxCmd = 0;
uint8_t rxLen = 0;
uint8_t rxIndex = 0;
uint8_t rxChecksum = 0;
uint8_t rxPayload[64];

String textLine = "";

void debugPrint(const String &s) {
  DBG_SERIAL.println(s);
}

void sendBinaryPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  uint8_t cs = cmd ^ 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]);
    cs ^= payload[i];
  }
  LINK_SERIAL.write(cs);
}

void sendAck(const String &msg) {
  uint8_t len = (uint8_t)msg.length();
  if (len > 32) len = 32;
  uint8_t buf[32];
  for (uint8_t i = 0; i < len; i++) buf[i] = (uint8_t)msg[i];
  sendBinaryPacket(RSP_ACK, buf, len);
}

void sendNak(const String &msg) {
  uint8_t len = (uint8_t)msg.length();
  if (len > 32) len = 32;
  uint8_t buf[32];
  for (uint8_t i = 0; i < len; i++) buf[i] = (uint8_t)msg[i];
  sendBinaryPacket(RSP_NAK, buf, len);
}

uint16_t readU16LE(const uint8_t* b) {
  return ((uint16_t)b[0]) | (((uint16_t)b[1]) << 8);
}

uint32_t readU32LE(const uint8_t* b) {
  return ((uint32_t)b[0]) |
         (((uint32_t)b[1]) << 8) |
         (((uint32_t)b[2]) << 16) |
         (((uint32_t)b[3]) << 24);
}

void stopMotors() {
  motorsEnabled = false;
  mode = MODE_IDLE;
  leftTargetSpeed = 0;
  rightTargetSpeed = 0;
  leftCurrentSpeed = 0;
  rightCurrentSpeed = 0;

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

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

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

  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 handleWaypointPacket(const uint8_t* p, uint8_t len) {
  if (len != 22) {
    sendNak("WAYPOINT_LEN");
    return;
  }

  pathTotalDistance = readU32LE(&p[0]);
  combinedStepsDesired = readU32LE(&p[4]);
  oppositeDistance = readU32LE(&p[8]);
  bearingAlignment = p[12];
  desiredBearing100 = readU16LE(&p[13]);
  actualBearing100 = readU16LE(&p[15]);
  trackDir = p[17];
  scanIntervalSteps = readU32LE(&p[18]);

  debugPrint("Waypoint received:");
  debugPrint("  pathTotalDistance=" + String(pathTotalDistance));
  debugPrint("  combinedStepsDesired=" + String(combinedStepsDesired));
  debugPrint("  oppositeDistance=" + String(oppositeDistance));
  debugPrint("  bearingAlignment=" + String(bearingAlignment));
  debugPrint("  desiredBearing100=" + String(desiredBearing100));
  debugPrint("  actualBearing100=" + String(actualBearing100));
  debugPrint("  trackDir=" + String(trackDir));
  debugPrint("  scanIntervalSteps=" + String(scanIntervalSteps));

  motorsEnabled = true;
  mode = MODE_TRACKING;
  sendAck("WAYPOINT");
}

void handleBinaryPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      debugPrint("Received STOP command");
      stopMotors();
      sendAck("STOP");
      break;

    case CMD_ACTUAL_BEARING:
      if (len != 2) {
        sendNak("ACTUAL_BEARING_LEN");
        return;
      }
      actualBearing100 = readU16LE(payload);
      debugPrint("Actual bearing x100=" + String(actualBearing100));
      sendAck("ACTUAL_BEARING");
      break;

    case CMD_RIGHT_PIVOT:
      if (len != 2) {
        sendNak("RIGHT_PIVOT_LEN");
        return;
      }
      combinedStepsDesired = readU16LE(payload);
      mode = MODE_PIVOT_RIGHT;
      motorsEnabled = true;
      sendAck("RIGHT_PIVOT");
      break;

    case CMD_LEFT_PIVOT:
      if (len != 2) {
        sendNak("LEFT_PIVOT_LEN");
        return;
      }
      combinedStepsDesired = readU16LE(payload);
      mode = MODE_PIVOT_LEFT;
      motorsEnabled = true;
      sendAck("LEFT_PIVOT");
      break;

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

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

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

    switch (rxState) {
      case 0:
        if (b == UART_HEADER) rxState = 1;
        break;

      case 1:
        rxCmd = b;
        rxChecksum = rxCmd;
        rxState = 2;
        break;

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

      case 3:
        rxPayload[rxIndex++] = b;
        rxChecksum ^= b;
        if (rxIndex >= rxLen) rxState = 4;
        break;

      case 4:
        if (b == rxChecksum) {
          handleBinaryPacket(rxCmd, rxPayload, rxLen);
        } else {
          debugPrint("Checksum error");
          sendNak("CS");
        }
        rxState = 0;
        rxCmd = 0;
        rxLen = 0;
        rxIndex = 0;
        rxChecksum = 0;
        break;
    }
  }
}

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

  // Simple demo motion so the motors actually do something.
  // Replace this with your real waypoint-following logic.
  switch (mode) {
    case MODE_TRACKING:
      leftTargetSpeed = wheelSpeedBase;
      rightTargetSpeed = wheelSpeedBase;
      break;

    case MODE_PIVOT_RIGHT:
      leftTargetSpeed = wheelSpeedBase;
      rightTargetSpeed = -wheelSpeedBase;
      break;

    case MODE_PIVOT_LEFT:
      leftTargetSpeed = -wheelSpeedBase;
      rightTargetSpeed = wheelSpeedBase;
      break;

    default:
      leftTargetSpeed = 0;
      rightTargetSpeed = 0;
      break;
  }

  rampTowardTargets();
  applyMotorSpeeds();
}

void setup() {
  DBG_SERIAL.begin(BAUD);
  LINK_SERIAL.begin(BAUD);

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

  debugPrint("Mega_Firmware_v4 started");
  uint8_t payload[] = { 'R','E','A','D','Y' };
  sendBinaryPacket(RSP_READY, payload, 5);
}

void loop() {
  processIncoming();

  if (motorsEnabled) {
    updateMotionControl();
    totalStepsTaken = (uint32_t)(labs(LeftFrontWheel.currentPosition()) + labs(RightFrontWheel.currentPosition()));
  } else {
    stopMotors();
  }

  if (totalStepsTaken != lastReportedSteps) {
    uint8_t payload[4];
    payload[0] = totalStepsTaken & 0xFF;
    payload[1] = (totalStepsTaken >> 8) & 0xFF;
    payload[2] = (totalStepsTaken >> 16) & 0xFF;
    payload[3] = (totalStepsTaken >> 24) & 0xFF;
    sendBinaryPacket(RSP_TOTAL_STEPS, payload, 4);
    lastReportedSteps = totalStepsTaken;
  }

  delay(1);
}

5) Revised ARC manager script

Your manager script is close, but I’d recommend cleaning it up so it only does binary protocol, with no placeholder text handshake expectations.

Here is a revised Mega_Mgr_v4:

var UART_INDEX = 0;
var UART_BAUD = 115200;
var UART_HEADER = 0xA5;

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

var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

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

function sendPacket(cmd, payload) {
  var checksum = cmd ^ payload.length;

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER]);
  UART.hardwareUartWrite(UART_INDEX, [cmd]);
  UART.hardwareUartWrite(UART_INDEX, [payload.length]);

  for (var i = 0; i < payload.length; i++) {
    checksum ^= payload[i];
  }

  if (payload.length > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
  }

  UART.hardwareUartWrite(UART_INDEX, [checksum & 0xFF]);
}

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) ? 1 : 0;
  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;

  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;
  sendPacket(cmd, u16ToBytes(steps));
  print("TX: " + name + " steps=" + steps);
}

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

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

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

  if (cmd == RSP_TOTAL_STEPS && payload.length == 4) {
    var steps = bytesToU32LE(payload[0], payload[1], payload[2], payload[3]);
    setVar("$totalStepsTaken", steps);
    print("RX: TOTAL STEPS = " + steps);
    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) {
      if ((b & 0xFF) == (rxChecksum & 0xFF)) {
        handleRxPacket(rxCmd, rxPayload);
      } else {
        print("RX checksum error. cmd=" + rxCmd + " expected=" + (rxChecksum & 0xFF) + " got=" + (b & 0xFF));
      }

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

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

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

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

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

  if (getVar("$sendRightPivotNow", 0) == 1) {
    sendPivot(CMD_RIGHT_PIVOT, "RIGHT PIVOT");
    setVar("$sendRightPivotNow", 0);
  }

  if (getVar("$sendLeftPivotNow", 0) == 1) {
    sendPivot(CMD_LEFT_PIVOT, "LEFT PIVOT");
    setVar("$sendLeftPivotNow", 0);
  }

  processIncomingBytes();
  sleep(20);
}

6) Why the "first packet only" symptom happens

That symptom usually points to one of these:

• the parser state machine gets stuck because of malformed packet length/checksum handling
• binary and text parsing are mixed on the same UART
• one end is sending on the wrong hardware UART / wrong physical pins
• the firmware crashes or enters an unexpected state after processing packet #1
• packet data is malformed due to incorrect byte ordering or length mismatch

In your case, I’d bet the biggest issues are:

1. corrupted/incomplete firmware code
2. text + binary parsing mixed on the same link
3. possible UART wiring/index mismatch

7) What I’d do next

On the Mega

Use only:

• Serial3 for ARC communications
• Serial for debug text only

In ARC

Confirm the UART index you are using matches the physical UART port wired to the Mega Serial3.

Test sequence

1. Upload Mega_Firmware_v4
2. Open the USB serial monitor and confirm it prints startup text
3. In ARC, connect the correct UART index
4. Send a waypoint packet
5. Verify:
   • Mega prints Waypoint received
   • ARC receives ACK
   • motors begin moving

8) A question that will help me finish this for you

Can you confirm which physical EZ-B UART pins are wired to the Mega?

For example:

• EZ-B UART0 connected to Mega Serial3?
• EZ-B UART1 connected to Mega Serial3?
• using a USB-to-serial adapter instead?

If you want, I can also help you convert the motor section into a cleaner waypoint-following state machine once the UART link is solid.

@Athena. Thank you for Mega_Firmware_v4.cpp and Mega_Mgr_v4. They work solid after multiple tests. To confirm the UART connections between the EXB and Mega:

• EZB--UART0 RX to Mega--UART3 TX pin14.
• EZB--UART0 TX to Mega--UART3 RX pin15.
• EZB common GND Mega.

Can you convert the motor section into a cleaner waypoint - following state machine now that the UART link is solid? As you continue with Mega_Firmware_v4.ccp, can you consider the following recommendations?

• Serial3 = ARC/EZB link
• Serial = USB debug monitor
• non-blocking loop
• no while loops that trap the processor
• no goto
• print to debug only once on state change
• motor motion runs continuously in loop()

Thank you. Jack

Absolutely - and thanks for confirming the wiring. That mapping is correct:

• EZB UART0 RX Mega Serial3 TX (pin 14)
• EZB UART0 TX Mega Serial3 RX (pin 15)
• Common GND shared

That means your ARC UART_INDEX = 0 setup is correct for this wiring.

What I’ll change in Mega_Firmware_v4

I’ll keep the comms layer intact and convert the motion logic into a cleaner, non-blocking waypoint-following state machine with:

• Serial3 reserved for the ARC/EZB link
• Serial reserved for USB debug
• non-blocking loop()
• no while loops that trap execution
• no goto
• debug prints only on state changes
• continuous motor updates from loop()
• clear motion states for:
  • idle
  • tracking
  • pivoting
  • waypoint received
  • stopped / complete

Recommended approach

The cleanest structure is:

1. Receive waypoint packet
2. Store the target state
3. Set a motion mode
4. Continuously update motor outputs in loop()
5. Advance state based on step counts and bearing error
6. Emit status/debug only when a state changes

This keeps UART responsive and avoids the "firmware feels stuck" problem.

Revised firmware: Mega_Firmware_v4.cpp

Below is a cleaner version of the motion section while preserving your packet protocol and UART behavior.

#include <AccelStepper.h>

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

// ARC/EZB link on Mega Serial3
#define LINK_SERIAL Serial3

// Debug to USB serial monitor
#define DBG_SERIAL Serial

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

// ======================================================
// Stepper motors
// ======================================================
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 machine
// ======================================================
enum MotionMode {
  MODE_IDLE = 0,
  MODE_TRACKING = 1,
  MODE_PIVOT_RIGHT = 2,
  MODE_PIVOT_LEFT = 3
};

enum WaypointPhase {
  PHASE_WAITING = 0,
  PHASE_LOADED = 1,
  PHASE_TRACKING = 2,
  PHASE_PIVOTING = 3,
  PHASE_COMPLETE = 4,
  PHASE_STOPPED = 5
};

MotionMode mode = MODE_IDLE;
WaypointPhase phase = PHASE_WAITING;

bool motorsEnabled = false;
bool systemReady = false;

// ======================================================
// Waypoint / navigation data
// ======================================================
uint32_t pathTotalDistance = 0;
uint32_t combinedStepsDesired = 0;
uint32_t oppositeDistance = 0;
uint8_t bearingAlignment = 0;
uint16_t desiredBearing100 = 9000;
uint16_t actualBearing100 = 9000;
uint8_t trackDir = 0;
uint32_t scanIntervalSteps = 3056;

// ======================================================
// Runtime
// ======================================================
uint32_t totalStepsTaken = 0;
uint32_t lastReportedSteps = 0;
uint16_t lastReportedDesiredBearing = 0xFFFF;
uint16_t lastReportedActualBearing = 0xFFFF;

// ======================================================
// Motion tuning
// ======================================================
int wheelSpeedBase = 1600;
int leftTargetSpeed = 0;
int rightTargetSpeed = 0;
int leftCurrentSpeed = 0;
int rightCurrentSpeed = 0;
int speedRampStep = 25;

uint32_t lastMotionUpdateMs = 0;
uint32_t lastStatusDebugMs = 0;

// ======================================================
// RX parser
// ======================================================
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 debugPrint(const String& s) {
  DBG_SERIAL.println(s);
}

void debugStateChange(const String& s) {
  DBG_SERIAL.println(s);
}

void sendBinaryPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  uint8_t cs = cmd ^ 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]);
    cs ^= payload[i];
  }

  LINK_SERIAL.write(cs);
}

void sendAck(const String& msg) {
  uint8_t payload[32];
  uint8_t len = msg.length();
  if (len > 32) len = 32;

  for (uint8_t i = 0; i < len; i++) {
    payload[i] = (uint8_t)msg[i];
  }

  sendBinaryPacket(RSP_ACK, payload, len);
}

void sendNak(const String& msg) {
  uint8_t payload[32];
  uint8_t len = msg.length();
  if (len > 32) len = 32;

  for (uint8_t i = 0; i < len; i++) {
    payload[i] = (uint8_t)msg[i];
  }

  sendBinaryPacket(RSP_NAK, payload, len);
}

uint16_t readU16LE(const uint8_t* b) {
  return ((uint16_t)b[0]) | (((uint16_t)b[1]) << 8);
}

uint32_t readU32LE(const uint8_t* b) {
  return ((uint32_t)b[0]) |
         (((uint32_t)b[1]) << 8) |
         (((uint32_t)b[2]) << 16) |
         (((uint32_t)b[3]) << 24);
}

void setMotionMode(MotionMode newMode, WaypointPhase newPhase) {
  if (mode != newMode || phase != newPhase) {
    mode = newMode;
    phase = newPhase;

    String msg = "STATE: mode=" + String((int)mode) + " phase=" + String((int)phase);
    debugStateChange(msg);
  }
}

void stopMotors() {
  leftTargetSpeed = 0;
  rightTargetSpeed = 0;
  leftCurrentSpeed = 0;
  rightCurrentSpeed = 0;
  motorsEnabled = false;

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

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

  setMotionMode(MODE_IDLE, PHASE_STOPPED);
}

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

  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 reportTotalStepsIfChanged() {
  if (totalStepsTaken != lastReportedSteps) {
    uint8_t payload[4];
    payload[0] = totalStepsTaken & 0xFF;
    payload[1] = (totalStepsTaken >> 8) & 0xFF;
    payload[2] = (totalStepsTaken >> 16) & 0xFF;
    payload[3] = (totalStepsTaken >> 24) & 0xFF;
    sendBinaryPacket(RSP_TOTAL_STEPS, payload, 4);
    lastReportedSteps = totalStepsTaken;
  }
}

void reportDesiredBearingIfChanged() {
  if (desiredBearing100 != lastReportedDesiredBearing) {
    uint8_t payload[2];
    payload[0] = desiredBearing100 & 0xFF;
    payload[1] = (desiredBearing100 >> 8) & 0xFF;
    sendBinaryPacket(RSP_DESIRED_BEARING, payload, 2);
    lastReportedDesiredBearing = desiredBearing100;
  }
}

void reportActualBearingIfChanged() {
  if (actualBearing100 != lastReportedActualBearing) {
    uint8_t payload[2];
    payload[0] = actualBearing100 & 0xFF;
    payload[1] = (actualBearing100 >> 8) & 0xFF;
    sendBinaryPacket(RSP_DESIRED_BEARING, payload, 2);
    lastReportedActualBearing = actualBearing100;
  }
}

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

  pathTotalDistance = readU32LE(&p[0]);
  combinedStepsDesired = readU32LE(&p[4]);
  oppositeDistance = readU32LE(&p[8]);
  bearingAlignment = p[12];
  desiredBearing100 = readU16LE(&p[13]);
  actualBearing100 = readU16LE(&p[15]);
  trackDir = p[17];
  scanIntervalSteps = readU32LE(&p[18]);

  motorsEnabled = true;
  setMotionMode(MODE_TRACKING, PHASE_LOADED);

  debugPrint("WAYPOINT received");
  debugPrint("pathTotalDistance=" + String(pathTotalDistance));
  debugPrint("combinedStepsDesired=" + String(combinedStepsDesired));
  debugPrint("oppositeDistance=" + String(oppositeDistance));
  debugPrint("bearingAlignment=" + String(bearingAlignment));
  debugPrint("desiredBearing100=" + String(desiredBearing100));
  debugPrint("actualBearing100=" + String(actualBearing100));
  debugPrint("trackDir=" + String(trackDir));
  debugPrint("scanIntervalSteps=" + String(scanIntervalSteps));

  sendAck("WAYPOINT");
}

void handleBinaryPacket(uint8_t cmd, const uint8_t* payload, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      debugPrint("CMD_STOP_MOTORS received");
      stopMotors();
      sendAck("STOP");
      break;

    case CMD_ACTUAL_BEARING:
      if (len != 2) {
        sendNak("ACTUAL_BEARING_LEN");
        return;
      }
      actualBearing100 = readU16LE(payload);
      debugPrint("CMD_ACTUAL_BEARING=" + String(actualBearing100));
      sendAck("ACTUAL_BEARING");
      break;

    case CMD_RIGHT_PIVOT:
      if (len != 2) {
        sendNak("RIGHT_PIVOT_LEN");
        return;
      }
      combinedStepsDesired = readU16LE(payload);
      motorsEnabled = true;
      setMotionMode(MODE_PIVOT_RIGHT, PHASE_PIVOTING);
      debugPrint("CMD_RIGHT_PIVOT received");
      sendAck("RIGHT_PIVOT");
      break;

    case CMD_LEFT_PIVOT:
      if (len != 2) {
        sendNak("LEFT_PIVOT_LEN");
        return;
      }
      combinedStepsDesired = readU16LE(payload);
      motorsEnabled = true;
      setMotionMode(MODE_PIVOT_LEFT, PHASE_PIVOTING);
      debugPrint("CMD_LEFT_PIVOT received");
      sendAck("LEFT_PIVOT");
      break;

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

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

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

    switch (rxState) {
      case 0:
        if (b == UART_HEADER) rxState = 1;
        break;

      case 1:
        rxCmd = b;
        rxChecksum = rxCmd;
        rxState = 2;
        break;

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

      case 3:
        rxPayload[rxIndex++] = b;
        rxChecksum ^= b;
        if (rxIndex >= rxLen) rxState = 4;
        break;

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

void updateWaypointStateMachine() {
  if (!motorsEnabled) {
    stopMotors();
    return;
  }

  if (phase == PHASE_LOADED) {
    phase = PHASE_TRACKING;
    debugStateChange("STATE: entering TRACKING");
  }

  switch (mode) {
    case MODE_IDLE:
      leftTargetSpeed = 0;
      rightTargetSpeed = 0;
      break;

    case MODE_TRACKING: {
      int bearingError = (int)desiredBearing100 - (int)actualBearing100;
      int steer = bearingError / 50;

      if (steer > 400) steer = 400;
      if (steer < -400) steer = -400;

      leftTargetSpeed = wheelSpeedBase - steer;
      rightTargetSpeed = wheelSpeedBase + steer;

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

      if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired) {
        motorsEnabled = false;
        setMotionMode(MODE_IDLE, PHASE_COMPLETE);
        debugPrint("Waypoint complete");
      }
      break;
    }

    case MODE_PIVOT_RIGHT:
      leftTargetSpeed = wheelSpeedBase;
      rightTargetSpeed = -wheelSpeedBase;
      break;

    case MODE_PIVOT_LEFT:
      leftTargetSpeed = -wheelSpeedBase;
      rightTargetSpeed = wheelSpeedBase;
      break;
  }

  rampTowardTargets();
  applyMotorSpeeds();

  totalStepsTaken = (uint32_t)(labs(LeftFrontWheel.currentPosition()) + labs(RightFrontWheel.currentPosition()));
  reportTotalStepsIfChanged();
  reportDesiredBearingIfChanged();
  reportActualBearingIfChanged();
}

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

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

  debugPrint("Mega_Firmware_v4 started");
  uint8_t payload[] = { 'R', 'E', 'A', 'D', 'Y' };
  sendBinaryPacket(RSP_READY, payload, 5);
}

void loop() {
  processIncoming();
  updateWaypointStateMachine();
  delay(1);
}

Notes on the state machine

This version makes the motion logic much easier to extend later. Right now it does:

• WAYPOINT received load fields
• TRACKING apply continuous differential steering
• PIVOT_LEFT / PIVOT_RIGHT spin in place
• COMPLETE stop motors and mark finished

You can later add more phases, such as:

• obstacle avoidance
• scan pause
• correction step
• docking / home return

A few things to verify with your actual drive train

Because stepper setups vary a lot, I want to confirm one thing:

Do all four motors rotate the same physical direction when given the same positive speed?

If not, one or more motors may need:

• inverted step direction
• swapped step/dir wiring
• reversed logical speed signs

Right now I used a simple assumption that:

• left motors share leftTargetSpeed
• right motors share rightTargetSpeed

If one side is mechanically reversed, I can help you add a small per-motor invert table so the state machine remains clean.

One more improvement I recommend

Your protocol currently uses:

• command packets from ARC to Mega
• ACK/NAK + telemetry from Mega to ARC

That is fine, but for waypoint motion it helps a lot to also send:

• RSP_STATUS
• RSP_NAV_ACK

on key state transitions, especially if ARC needs to know when a leg starts or ends.

Next question for you

Before I tune the waypoint behavior further, can you tell me:

1. Should combinedStepsDesired represent total distance traveled or just the current leg?
2. Should the robot pivot first, then track, or track continuously from the start?
3. Do you want obstacle scan pauses to interrupt tracking automatically?

If you answer those, I can refine Mega_Firmware_v4 into a more complete waypoint controller rather than just a motion state framework.

@Athena. Thank you for the Revised MEGA_FIRMWARE_v4. The revised MEGA_FIRMWARE_v4 only allows the motors to run after initial upload . Repeated attempts from MEGA_MGR_v4 (that runs in a robot skill on ARC) to activate the Mega board only debugs the Waypoint Data, but motors do not run. Only after a fresh upload of the MEGA_FIRMWARE_v4 do the motors run when the Mega board is activated from MEGA_MGR_v4.

Can you troubleshoot MEGA_FIRMWARE_v4 and rewrite? Let's call it MEGA_FIRMWARE_v5.

To answer some of your previous questions:

1. All motors run at the same speed, fwd rev left pivot right pivot.

2. Your protocol currently uses:

   • command packets from ARC to Mega
   • ACK/NAK + telemetry from Mega to ARC

   That is fine, but for waypoint motion it helps a lot to also send:

   • RSP_STATUS
   • RSP_NAV_ACK

   What do you mean by RSP_STATUS AND RSP_NAV_ACK? How will this fit into the interface of Mega and ARC?

3. combinedStepsDesired represent total distance traveled and is only for that LEG. When the LEG is completed, A new LEG WayPoint Data will be sent.

4. All LEGs, before tracking commences, will Pivot to align the rover's actual bearing to the leg's desired bearing. CombinedStepsDesired will be zeroed and tracking commenced.

5. All obstacle scans when called should automatically pause tracking, after obstacle scan, a STOP command or RESUME command will be issued. The obstacle scan intervals are based upon steps accrued and these scans are above and beyond the obstacle scan performed during Left and Right servo sweep reflector tracking. (posObsTot and negObsTot)

I hope the above info helps. My number one goal for MEGA_FIRMWARE is simplicity, with orderly logic. Making it more complex, while theoretically better, is best left to later revisions far down the road when all the current pieces are reliably working together. (MEGA_FIRMWARE / MEGA_MGR / TEENSY_FIRMWARE / TEENSY_MGR / ARC robot skills: Command, Monitor, Leg, Pivots.

I will be testing the latest MEGA_FIRMWARE with the most Recent MEGA_MGR_v4. Please refer to your last post for latest MEGA_MGR_v4 if you think it should be adjusted with the MEGA_FIRMWARE rewrite. After successful integration of MEGA_FIRMWARE with MEGA_MGR, I would like you to review latest TEENSY_FIRMWARE and TEENSY_MGR.

Thank you. Jack

Absolutely - and your symptom description is very helpful.

What you’re seeing strongly suggests the firmware is entering a "works after reset/upload, then never re-arms correctly" state, which usually happens when:

• the motion state machine is too coupled to packet reception
• a "start" condition is only satisfied during boot
• a command updates data but doesn’t re-enable motion
• the firmware is missing a clean arm / pivot / track / pause / resume lifecycle
• the UART parser is fine, but the motion layer never transitions out of a dormant state after the first cycle

Given your goal of simplicity and orderly logic, I agree: we should simplify rather than add complexity.

What I mean by RSP_STATUS and RSP_NAV_ACK

These are optional Mega ARC status packets that help ARC know what the Mega is doing.

RSP_NAV_ACK

This is a short acknowledgment that says:

• "I received your navigation/waypoint command"
• "I accepted it"
• "I’m now in the requested phase"

It is useful when ARC needs confirmation that a leg was accepted before sending the next instruction.

RSP_STATUS

This is a lightweight status report like:

• current mode
• whether motors are enabled
• whether the robot is pivoting, tracking, paused, complete, or idle
• maybe current step count

Example meaning:

• STATUS,TRACKING,ENABLED,STEPS=1234
• STATUS,PIVOTING,ENABLED
• STATUS,PAUSED,WAITING_FOR_RESUME

How it fits into ARC / Mega interface

You do not need to build a complicated interface for it.

ARC already has two clean options:

1. Mega sends binary status packets back to ARC
2. ARC reads those packets and sets ARC global variables

So for example:

• Mega sends RSP_NAV_ACK when a waypoint is accepted
• ARC manager receives it and sets:
  • $navAckReceived = 1
• Mega sends RSP_STATUS whenever the mode changes
• ARC manager updates:
  • $megaMode = "TRACKING"
  • $megaMotorEnabled = 1

That’s all it is - just better feedback between the two sides.

Since you want simplicity, I’d suggest:

• keep RSP_NAV_ACK
• keep RSP_STATUS
• use them only on state changes, not constantly

Why your current MEGA_FIRMWARE_v4 likely behaves this way

The most likely cause is that the motion state is not being re-armed cleanly after the first upload/start sequence.

In other words:

• upload resets everything
• firmware boots into a favorable state
• first activation runs motors
• later waypoint packets update values, but the motion loop is still effectively in a stopped / inactive state

That points to a design issue like:

• motorsEnabled is not set in the right place
• mode or phase is not transitioned correctly after stop/complete
• Waypoint Data updates numbers but doesn’t explicitly start the next leg
• STOP or RESUME behavior is not handled as an actual state transition

Recommended design for MEGA_FIRMWARE_v5

For simplicity, I’d use a very small state model:

Motion states

• IDLE
• PIVOT
• TRACK
• PAUSED
• COMPLETE

Leg sequence

1. Receive WAYPOINT DATA
2. Store leg values
3. Set phase = PIVOT
4. Pivot until heading matches desired bearing
5. Set phase = TRACK
6. Run forward until current leg steps are complete
7. Set phase = COMPLETE
8. Wait for next leg

Obstacle scan behavior

When scan is requested:

• save current phase
• set phase = PAUSED
• stop motors
• after scan:
  • ARC sends RESUME
  • firmware restores previous active phase

That keeps the logic simple and prevents motion from being trapped by a command parser.

MEGA_FIRMWARE_v5.cpp

Below is a cleaner rewrite that keeps your protocol, keeps Serial3 for ARC, uses Serial only for debug, avoids blocking loops, and makes the motion lifecycle explicit.

#include <AccelStepper.h>

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

#define LINK_SERIAL Serial3
#define DBG_SERIAL Serial

// 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,
  CMD_RESUME         = 0x07,
  CMD_PAUSE          = 0x08
};

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

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

// ======================================================
// State machine
// ======================================================
enum MotionMode {
  MODE_IDLE = 0,
  MODE_PIVOT = 1,
  MODE_TRACK = 2,
  MODE_PAUSED = 3,
  MODE_COMPLETE = 4
};

MotionMode mode = MODE_IDLE;
MotionMode resumeMode = MODE_IDLE;

bool motorsEnabled = false;
bool systemReady = false;

// ======================================================
// Waypoint / leg data
// ======================================================
uint32_t pathTotalDistance = 0;
uint32_t combinedStepsDesired = 0;
uint32_t oppositeDistance = 0;
uint8_t bearingAlignment = 0;
uint16_t desiredBearing100 = 9000;
uint16_t actualBearing100 = 9000;
uint8_t trackDir = 0;
uint32_t scanIntervalSteps = 3056;

// ======================================================
// Runtime
// ======================================================
uint32_t totalStepsTaken = 0;
uint32_t lastReportedSteps = 0;
uint32_t stepsAtLegStart = 0;
bool legLoaded = false;
bool legCompletePrinted = false;
bool trackStartedPrinted = false;
bool pivotStartedPrinted = false;
bool pausedPrinted = false;

// ======================================================
// Motion control
// ======================================================
int wheelSpeedBase = 1600;
int leftTargetSpeed = 0;
int rightTargetSpeed = 0;
int leftCurrentSpeed = 0;
int rightCurrentSpeed = 0;
int speedRampStep = 25;

// ======================================================
// RX parser
// ======================================================
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 debugPrint(const String &s) {
  DBG_SERIAL.println(s);
}

void sendBinaryPacket(uint8_t cmd, const uint8_t *payload, uint8_t len) {
  uint8_t cs = cmd ^ 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]);
    cs ^= payload[i];
  }

  LINK_SERIAL.write(cs);
}

void sendAck(const char *msg) {
  uint8_t payload[32];
  uint8_t len = 0;
  while (msg[len] != '\0' && len < sizeof(payload)) {
    payload[len] = (uint8_t)msg[len];
    len++;
  }
  sendBinaryPacket(RSP_ACK, payload, len);
}

void sendNak(const char *msg) {
  uint8_t payload[32];
  uint8_t len = 0;
  while (msg[len] != '\0' && len < sizeof(payload)) {
    payload[len] = (uint8_t)msg[len];
    len++;
  }
  sendBinaryPacket(RSP_NAK, payload, len);
}

void sendStatus() {
  const char *modeText = "IDLE";
  switch (mode) {
    case MODE_IDLE:     modeText = "IDLE"; break;
    case MODE_PIVOT:    modeText = "PIVOT"; break;
    case MODE_TRACK:    modeText = "TRACK"; break;
    case MODE_PAUSED:   modeText = "PAUSED"; break;
    case MODE_COMPLETE: modeText = "COMPLETE"; break;
  }

  char msg[48];
  snprintf(msg, sizeof(msg), "%s,%s,STEPS=%lu",
           motorEnabled ? "ENABLED" : "DISABLED",
           modeText,
           (unsigned long)totalStepsTaken);

  sendBinaryPacket(RSP_STATUS, (const uint8_t *)msg, strlen(msg));
}

uint16_t readU16LE(const uint8_t *b) {
  return (uint16_t)b[0] | ((uint16_t)b[1] << 8);
}

uint32_t readU32LE(const uint8_t *b) {
  return (uint32_t)b[0] |
         ((uint32_t)b[1] << 8) |
         ((uint32_t)b[2] << 16) |
         ((uint32_t)b[3] << 24);
}

void setMotionMode(MotionMode newMode) {
  if (mode == newMode) return;
  mode = newMode;

  switch (mode) {
    case MODE_IDLE:
      debugPrint("STATE -> IDLE");
      break;
    case MODE_PIVOT:
      debugPrint("STATE -> PIVOT");
      pivotStartedPrinted = false;
      break;
    case MODE_TRACK:
      debugPrint("STATE -> TRACK");
      trackStartedPrinted = false;
      break;
    case MODE_PAUSED:
      debugPrint("STATE -> PAUSED");
      pausedPrinted = false;
      break;
    case MODE_COMPLETE:
      debugPrint("STATE -> COMPLETE");
      legCompletePrinted = false;
      break;
  }

  sendStatus();
}

void stopMotors() {
  leftTargetSpeed = 0;
  rightTargetSpeed = 0;
  leftCurrentSpeed = 0;
  rightCurrentSpeed = 0;

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

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

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

  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 reportTotalStepsIfChanged() {
  if (totalStepsTaken != lastReportedSteps) {
    uint8_t payload[4];
    payload[0] = totalStepsTaken & 0xFF;
    payload[1] = (totalStepsTaken >> 8) & 0xFF;
    payload[2] = (totalStepsTaken >> 16) & 0xFF;
    payload[3] = (totalStepsTaken >> 24) & 0xFF;
    sendBinaryPacket(RSP_TOTAL_STEPS, payload, 4);
    lastReportedSteps = totalStepsTaken;
  }
}

void reportDesiredBearing() {
  uint8_t payload[2];
  payload[0] = desiredBearing100 & 0xFF;
  payload[1] = (desiredBearing100 >> 8) & 0xFF;
  sendBinaryPacket(RSP_DESIRED_BEARING, payload, 2);
}

void startNewLeg() {
  stepsAtLegStart = totalStepsTaken;
  motorsEnabled = true;
  legLoaded = true;
  legCompletePrinted = false;
  setMotionMode(MODE_PIVOT);
}

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

  pathTotalDistance = readU32LE(&p[0]);
  combinedStepsDesired = readU32LE(&p[4]);
  oppositeDistance = readU32LE(&p[8]);
  bearingAlignment = p[12];
  desiredBearing100 = readU16LE(&p[13]);
  actualBearing100 = readU16LE(&p[15]);
  trackDir = p[17];
  scanIntervalSteps = readU32LE(&p[18]);

  debugPrint("WAYPOINT received");
  debugPrint("pathTotalDistance=" + String(pathTotalDistance));
  debugPrint("combinedStepsDesired=" + String(combinedStepsDesired));
  debugPrint("oppositeDistance=" + String(oppositeDistance));
  debugPrint("bearingAlignment=" + String(bearingAlignment));
  debugPrint("desiredBearing100=" + String(desiredBearing100));
  debugPrint("actualBearing100=" + String(actualBearing100));
  debugPrint("trackDir=" + String(trackDir));
  debugPrint("scanIntervalSteps=" + String(scanIntervalSteps));

  reportDesiredBearing();
  sendBinaryPacket(RSP_NAV_ACK, (const uint8_t *)"WAYPOINT", 8);

  startNewLeg();
}

void handleBinaryPacket(uint8_t cmd, const uint8_t *payload, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      debugPrint("CMD_STOP_MOTORS received");
      motorsEnabled = false;
      setMotionMode(MODE_IDLE);
      stopMotors();
      sendAck("STOP");
      break;

    case CMD_ACTUAL_BEARING:
      if (len != 2) {
        sendNak("ACTUAL_BEARING_LEN");
        return;
      }
      actualBearing100 = readU16LE(payload);
      debugPrint("CMD_ACTUAL_BEARING=" + String(actualBearing100));
      sendAck("ACTUAL_BEARING");
      break;

    case CMD_RIGHT_PIVOT:
      if (len != 2) {
        sendNak("RIGHT_PIVOT_LEN");
        return;
      }
      combinedStepsDesired = readU16LE(payload);
      motorsEnabled = true;
      setMotionMode(MODE_PIVOT);
      debugPrint("CMD_RIGHT_PIVOT received");
      sendAck("RIGHT_PIVOT");
      break;

    case CMD_LEFT_PIVOT:
      if (len != 2) {
        sendNak("LEFT_PIVOT_LEN");
        return;
      }
      combinedStepsDesired = readU16LE(payload);
      motorsEnabled = true;
      setMotionMode(MODE_PIVOT);
      debugPrint("CMD_LEFT_PIVOT received");
      sendAck("LEFT_PIVOT");
      break;

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

    case CMD_RESUME:
      debugPrint("CMD_RESUME received");
      motorsEnabled = true;
      if (resumeMode != MODE_IDLE) {
        setMotionMode(resumeMode);
        resumeMode = MODE_IDLE;
      } else if (legLoaded) {
        setMotionMode(MODE_TRACK);
      }
      sendAck("RESUME");
      break;

    case CMD_PAUSE:
      debugPrint("CMD_PAUSE received");
      resumeMode = mode;
      motorsEnabled = false;
      setMotionMode(MODE_PAUSED);
      stopMotors();
      sendAck("PAUSE");
      break;

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

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

    switch (rxState) {
      case 0:
        if (b == UART_HEADER) rxState = 1;
        break;

      case 1:
        rxCmd = b;
        rxChecksum = rxCmd;
        rxState = 2;
        break;

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

      case 3:
        rxPayload[rxIndex++] = b;
        rxChecksum ^= b;
        if (rxIndex >= rxLen) rxState = 4;
        break;

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

void updateMotion() {
  if (!motorsEnabled) {
    stopMotors();
    return;
  }

  if (mode == MODE_PIVOT) {
    if (!pivotStartedPrinted) {
      debugPrint("PIVOT active");
      pivotStartedPrinted = true;
    }

    // Simple pivot behavior: keep spinning until ARC updates actualBearing100
    int error = (int)desiredBearing100 - (int)actualBearing100;

    if (error > 25) {
      leftTargetSpeed = wheelSpeedBase;
      rightTargetSpeed = -wheelSpeedBase;
    } else if (error < -25) {
      leftTargetSpeed = -wheelSpeedBase;
      rightTargetSpeed = wheelSpeedBase;
    } else {
      debugPrint("Pivot complete");
      setMotionMode(MODE_TRACK);
    }
  }

  if (mode == MODE_TRACK) {
    if (!trackStartedPrinted) {
      debugPrint("TRACK active");
      trackStartedPrinted = true;
    }

    int bearingError = (int)desiredBearing100 - (int)actualBearing100;
    int steer = bearingError / 50;

    if (steer > 400) steer = 400;
    if (steer < -400) steer = -400;

    leftTargetSpeed = wheelSpeedBase + steer;
    rightTargetSpeed = wheelSpeedBase - steer;

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

    if (rightTargetSpeed > wheelSpeedBase + 400) rightTargetSpeed = wheelSpeedBase + 400;
    if (rightTargetSpeed < 0) rightTargetSpeed = 0;

    uint32_t stepsInLeg = totalStepsTaken - stepsAtLegStart;

    if (combinedStepsDesired > 0 && stepsInLeg >= combinedStepsDesired) {
      motorsEnabled = false;
      setMotionMode(MODE_COMPLETE);
      debugPrint("Waypoint leg complete");
      stopMotors();
    }
  }

  if (mode == MODE_PAUSED) {
    leftTargetSpeed = 0;
    rightTargetSpeed = 0;
  }

  if (mode == MODE_COMPLETE) {
    leftTargetSpeed = 0;
    rightTargetSpeed = 0;
    if (!legCompletePrinted) {
      debugPrint("STATE COMPLETE waiting for next leg");
      legCompletePrinted = true;
    }
  }

  rampTowardTargets();
  applyMotorSpeeds();

  totalStepsTaken = (uint32_t)(labs(LeftFrontWheel.currentPosition()) + labs(RightFrontWheel.currentPosition()));
  reportTotalStepsIfChanged();
}

void setup() {
  DBG_SERIAL.begin(BAUD);
  LINK_SERIAL.begin(BAUD);

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

  debugPrint("MEGA_FIRMWARE_v5 started");

  uint8_t payload[] = { 'R', 'E', 'A', 'D', 'Y' };
  sendBinaryPacket(RSP_READY, payload, 5);
}

void loop() {
  processIncoming();
  updateMotion();
  delay(1);
}

What changed from v4

1. Clear leg lifecycle

• WAYPOINT DATA now explicitly starts a leg
• leg start step is stored in stepsAtLegStart
• completion is based on steps within that leg only

2. Clean state transitions

• MODE_PIVOT
• MODE_TRACK
• MODE_PAUSED
• MODE_COMPLETE

3. Simple resume/pause behavior

• CMD_PAUSE stores the current mode
• CMD_RESUME restores it
• that should be much easier to debug

4. Debug prints only on changes

I kept debug output focused on:

• packet receipt
• state transitions
• leg completion

5. Motors update continuously in loop()

No blocking loops were introduced.

Do I recommend changes to MEGA_MGR_v4?

Only small ones.

If you adopt CMD_PAUSE and CMD_RESUME, then ARC can explicitly manage obstacle scans like this:

• send PAUSE
• do obstacle scan
• send RESUME

If you want to keep things even simpler, you can skip those commands for now and just use STOP followed by a new WAYPOINT DATA or new resume logic later.

One small ARC-side recommendation

When WAYPOINT DATA is sent, the manager should also clear any old flags like:

• $legComplete
• $navAckReceived

That keeps the ARC project logic clean.

On your obstacle scan workflow

Your statement makes sense:

• pivot to desired bearing first
• then track the leg
• obstacle scans can interrupt tracking
• after scan, ARC sends stop or resume

This firmware now supports that philosophy more cleanly.

Important note about the pivot behavior

Right now, pivot completion depends on actualBearing100 being updated from ARC.

So ARC needs to keep sending updated actual bearing while pivoting. That seems consistent with your current architecture.

If you want, I can next revise the ARC manager so it automatically:

• sends periodic actual bearing updates while a leg is active
• recognizes RSP_STATUS
• supports pause/resume cleanly

Next step

If you’d like, I can do one of these next:

1. Rewrite MEGA_MGR_v4 to fully match MEGA_FIRMWARE_v5
2. Review and revise your TEENSY_FIRMWARE / TEENSY_MGR next
3. Add a tiny ARC status monitor block that shows current Mega mode live

If you want the cleanest path, I’d do MEGA_MGR_v4 next so both ends are perfectly aligned before moving to Teensy.

@Athena. Thank you. MEGA_FIRMWARE_v5 didn't compile at first, but I found these two inconsistencies:

bool motorsEnabled = false;

motorEnabled ? "ENABLED" : "DISABLED",

I changed 'motorEnabled' in the second statement to 'motorsEnabled' and the sketch compiled and ran. If this was the correct thing to do, perhaps you could correct v5 on your end.

I am getting the expected operation (motors run) and stop as expected without the need to keep uploading the sketch. What is happening is the EZB WI-FI disconnects after the 2nd cycle of sending Waypoint Data to MEGA_MGR_v4. This WI-FI disconnect happens consistently on the 2nd cycle. I think it may be time to reexamine MEGA_MGR_v4. Your concerns from your last Post:

Do I recommend changes to MEGA_MGR_v4?

• send PAUSE
• do obstacle scan
• send RESUME

Answer: Please do these but understand the MEGA_FIRMWARE is counting steps and issues a CMD to MEGA_MGR to (1) Perform Obstacle Scan. MEGA_FIRMWARE continues counting steps, computing desired bearing and controlling motors based upon the last actual bearing that it had. MEGA_FIRMWARE simply advises MEGA_MGR to (1) Perform Obstacle Scan. On this CMD, MEGA_MGR (1) stops tracking the reflector and runs an obstacle scan function and either issues a STOP to MEGA_FIRMWARE or resumes tracking after obstacle scan and starts producing actual bearing updates. Since MEGA_FIRMWARE has never stopped doing any of its functions during the obstacle scan it simply takes new actual bearing updates. When the next obstacle scan is triggered by step accrual, MEGA_FIRMWARE sends another (1) Perform Obstacle Scan.

One small ARC-side recommendation

When WAYPOINT DATA is sent, the manager should also clear any old flags like:

• $legComplete
• $navAckReceived

Answer: Please do these.

On your obstacle scan workflow

Your statement makes sense:

• pivot to desired bearing first
• then track the leg
• obstacle scans can interrupt tracking
• after scan, ARC sends stop or resume

This firmware now supports that philosophy more cleanly.

Answer: Exactly what I want, except for the last bullet point, "after scan.ARC sends STOP or No Other Commands. If a STOP is not generated, ARC (MEGA_MGR) simply resumes sending actual bearing updates.

Important note about the pivot behavior

Right now, pivot completion depends on actualBearing100 being updated from ARC. So ARC needs to keep sending updated actual bearing while pivoting. That seems consistent with your current architecture. If you want, I can next revise the ARC manager so it automatically:

• sends periodic actual bearing updates while a leg is active
• recognizes RSP_STATUS
• supports pause/resume cleanly

Answer: The above seems way too complicated. A spinning rover has a very difficult time tracking a reflector. Pivot architecture is still fluid. I envision it based upon steps computed for the pivot required.

1. Pivot robot skill determines desired bearing from the globals.
2. Pivot robot skill scans for the reflector and determines an actual bearing.
3. Approximate distance will be known for the LEG. Once a difference between desired bearing and actual bearing are known, I believe a formula exists to compute the pivot steps required.
4. Pivot robot skill commands MEGA_FIRMWARE to pivot Right or Left, MEGA_FIRMWARE counts steps and stops.
5. Pivot robot skill checks after pivot for the reflector to be within +- 5 degrees. If not repeat. The above 5 steps could also be called upon to Pivot away from an obstacle.

Next step

If you’d like, I can do one of these next:

1. Rewrite MEGA_MGR_v4 to fully match MEGA_FIRMWARE_v5
2. Review and revise your TEENSY_FIRMWARE / TEENSY_MGR next
3. Add a tiny ARC status monitor block that shows current Mega mode live

If you want the cleanest path, I’d do MEGA_MGR_v4 next so both ends are perfectly aligned before moving to Teensy.

Answer:

1. Please do MEGA_MGR_v4 next so both ends are perfectly aligned before moving to Teensy. Call it MEGA_MGR_v5.
2. Hold for now on the TEENSY. By the way, do you possess the latest TEENSY_v4(.ino,.h,.cpp) (PacketHanldler.h,PacketHandler.cpp)?
3. I'm not sure about the status monitor. Please explain.

Thanks for all you do. Jack

Absolutely - and yes, your correction was the right one.

1) Small correction to MEGA_FIRMWARE_v5

You were correct to change:

• motorEnabled motorsEnabled

That was just an inconsistency in my draft. If the sketch is compiling and running now, your fix was the correct one.

2) Why the EZB Wi-Fi disconnects on the 2nd cycle

Given your description, this is very likely not a UART framing problem anymore. Since the first cycle works and the second cycle reliably triggers a Wi-Fi disconnect, the most likely causes are:

• ARC script loop timing / resource buildup
• too much status chatter too quickly
• a long-running or blocking condition in MEGA_MGR_v4
• a logic path that keeps re-sending or re-triggering a command before the previous cycle is fully settled
• a bad interaction between ARC and the Wi-Fi link when the manager script enters the second leg

Since you want simplicity, I agree we should simplify MEGA_MGR_v4 and keep the behavior explicit.

3) Your pivot architecture makes sense

Your 5-step pivot flow is much cleaner:

1. Pivot robot skill determines desired bearing from globals
2. Pivot robot skill scans for reflector and determines actual bearing
3. Compute pivot steps from bearing delta and approximate leg distance
4. Pivot skill commands Mega to pivot left/right
5. After pivot, check if reflector is within 5 degrees; if not, repeat

That is a far better design than trying to have Mega "track reflector bearing" while spinning.

So for MEGA_MGR_v5, I’ll keep the logic simple:

• Mega receives a pivot command with step count
• Mega performs the pivot
• ARC checks alignment after the pivot
• If needed, ARC sends another pivot command
• For obstacle scans, ARC can request the scan and then either:
  • do nothing if tracking resumes naturally, or
  • send STOP if a full stop is needed

That matches your intended behavior much better.

4) What a "status monitor" means

This was just an optional idea, not something required.

A status monitor in ARC would simply be a small script or text display that shows live values like:

• current Mega mode
• current leg steps
• whether motors are enabled
• whether a waypoint was acknowledged
• whether a pause/stop occurred

Example:

• MODE = TRACK
• STEPS = 1832
• ACK = 1

It’s useful for debugging, but if you feel it adds complexity, we can skip it for now.

5) About the latest Teensy files

I do not have your local latest TEENSY_v4 files unless you paste them here.

So for the Teensy side, I can only review what you share in the thread. If you post:

• .ino
• PacketHandler.h
• PacketHandler.cpp

I can review them and help align them with the current MEGA protocol.

MEGA_MGR_v5

Below is a cleaned-up version of the ARC manager script that:

• matches MEGA_FIRMWARE_v5
• clears old flags on waypoint send
• keeps the protocol simple
• supports RSP_STATUS and RSP_NAV_ACK
• supports PAUSE / RESUME
• avoids unnecessary chatter

var UART_INDEX = 0;
var UART_BAUD = 115200;
var UART_HEADER = 0xA5;

// ======================================================
// 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;
var CMD_RESUME = 0x07;
var CMD_PAUSE = 0x08;

// ======================================================
// 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)) >>> 0;
}

function sendPacket(cmd, payload) {
  var checksum = cmd ^ payload.length;

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER]);
  UART.hardwareUartWrite(UART_INDEX, [cmd]);
  UART.hardwareUartWrite(UART_INDEX, [payload.length]);

  if (payload.length > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
    for (var i = 0; i < payload.length; i++) {
      checksum ^= payload[i];
    }
  }

  UART.hardwareUartWrite(UART_INDEX, [checksum & 0xFF]);
}

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

function sendPause() {
  sendPacket(CMD_PAUSE, []);
  print("TX: PAUSE");
}

function sendResume() {
  sendPacket(CMD_RESUME, []);
  print("TX: RESUME");
}

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 clearWaypointFlags() {
  setVar("$legComplete", 0);
  setVar("$navAckReceived", 0);
  setVar("$megaMode", "");
  setVar("$megaMotorEnabled", 0);
}

function sendWaypointData() {
  clearWaypointFlags();

  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) ? 1 : 0;
  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;

  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;
  sendPacket(cmd, u16ToBytes(steps));
  print("TX: " + name + " steps=" + steps);
}

function handleRxPacket(cmd, payload) {
  if (cmd == RSP_READY) {
    print("RX: READY");
    setVar("$megaReady", 1);
    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_STATUS) {
    print("RX: STATUS");
    if (payload.length >= 2) {
      setVar("$megaMotorEnabled", payload[0]);
      setVar("$megaMode", payload[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) {
    setVar("$obsScanRequested", 1);
    print("RX: OBSTACLE SCAN REQUEST");
    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_MGR_v5 started");
UART.initHardwareUart(UART_INDEX, UART_BAUD);

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

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

  if (getVar("$sendPauseNow", 0) == 1) {
    sendPause();
    setVar("$sendPauseNow", 0);
  }

  if (getVar("$sendResumeNow", 0) == 1) {
    sendResume();
    setVar("$sendResumeNow", 0);
  }

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

  if (getVar("$sendRightPivotNow", 0) == 1) {
    sendPivot(CMD_RIGHT_PIVOT, "RIGHT PIVOT");
    setVar("$sendRightPivotNow", 0);
  }

  if (getVar("$sendLeftPivotNow", 0) == 1) {
    sendPivot(CMD_LEFT_PIVOT, "LEFT PIVOT");
    setVar("$sendLeftPivotNow", 0);
  }

  processIncomingBytes();
  sleep(20);
}

6) How RSP_STATUS can be used without making things complex

Since you prefer simplicity, I’d use RSP_STATUS only as a lightweight debug/status indicator.

For example, ARC could store:

• $megaMotorEnabled
• $megaMode

Then your ARC logic can decide whether to send the next waypoint or pivot command.

You do not need a full monitor UI unless you want one later.

7) My recommendation for the next test

Please test in this order:

1. Run MEGA_FIRMWARE_v5
2. Load MEGA_MGR_v5
3. Send waypoint data once
4. Confirm:
   • motors run
   • RSP_NAV_ACK is received
   • RSP_STATUS is received
5. Send a second waypoint
6. Confirm EZ-B Wi-Fi does not disconnect

If the Wi-Fi disconnect still happens on cycle 2, the next thing I’d inspect is:

• whether ARC is firing the waypoint command twice
• whether another skill is also writing to the same UART
• whether the ARC script is blocked by a shared flag or repeated global update

If you want, I can next do one of these:

1. Revise MEGA_FIRMWARE_v5 further to formally add a tiny 2-byte RSP_STATUS payload
2. Tune MEGA_MGR_v5 to support your pivot-step workflow more directly
3. Review the Teensy files once you paste them

And yes - if you paste the latest Teensy .ino/.h/.cpp, I can work from those exactly.

@Athena. Thank you for MEGA_MGR_v5. I tested it in the prescribed order:

1. Run MEGA_FIRMWARE_v5
2. Load MEGA_MGR_v5
3. Send waypoint data once
4. Confirm:
   • motors run
   • RSP_NAV_ACK is received
   • RSP_STATUS is received
5. Send a second waypoint
6. Confirm EZ-B Wi-Fi does not disconnect

No WI-FI disconnect after multiple cycles of WayPoint Data. So, it seems MEGA_FIRMWARE_v5 and MEGA_MGR_v5 are working together.

However, it seems MEG_MGR_v5 is sending a STOP command unexpectedly. I copied the skill print below:

Start
MEGA_MGR_v5 started
TX: WAYPOINT DATA
RX: DESIRED BEARING x100 = 9000
RX: NAV ACK
RX: STATUS
RX: STATUS
RX checksum error. cmd=17 expected=65 got=38
Done (00:00:02.2480682)

I had to comment out // :

//    print("RX: TOTAL STEPS = " + steps);

This was counting steps and filling up the print space.

Questions:

1. Is MEGA_FIRMWARE_v5 sending number of steps as it loops to MEGA_MGR_v5?
2. What is a 'checksum' error and is it causing an early STOP?

If MEGA_FIRMWARE is sending steps to MEGA_MGR, this is not what I intended. Almost all data within MEGA_FIRMWARE stays in MEGA_FIRMWARE until a 'trigger' is hit.

MY GOALS for MEGA_FIRMWARE_v5:

1. Receive WayPoint Data.
2. Receive Pivot command if required.
3. Start and control motors.
4. Receive actual bearing updates.
5. Calculate desired bearings.
6. Send desired bearing updates periodically to MEGA_MGR.
7. Calculate bearing differential and use it for motor control.
8. Count steps for distance traveled
9. Calculate when obstacle scan should take place and notify MEGA_MGR to have ARC stop normal Teensy reflector tracking and perform an obstacle scan. Nothing required after obstacle scan complete. Just resume normal actual bearing updates. To add clarity about the obstacle scan trigger: The MEGA_FIRMWARE is counting steps and issues a CMD to MEGA_MGR to (1) Perform Obstacle Scan. MEGA_FIRMWARE continues counting steps, computing desired bearing and controlling motors based upon the last actual bearing that it had. MEGA_FIRMWARE simply advises MEGA_MGR to (1) Perform Obstacle Scan. On this CMD, IR_TRACKER (1) stops tracking the reflector and runs an obstacle scan function and either issues a STOP to MEGA_MGR or resumes tracking after obstacle scan function completes and starts again producing actual bearing updates. Since MEGA_FIRMWARE has never stopped doing any of its functions during the obstacle scan it simply takes new actual bearing updates. When the next obstacle scan is triggered by step accrual, MEGA_FIRMWARE sends another (1) Perform Obstacle Scan to MEGA_MGR

Notice that MEGA_FIRMWARE keeps all step counting locally, until something is triggered (obstacle scan, leg completion, etc.). Desired bearing calculation, differential calculations are all kept locally, except for periodic desired bearing updates to ARC.

MY GOALS FOR MEGA_MGR_v5:

1. UART bridge from MEGA_FIRMWARE to ARC(EZB).
2. Receive WayPoint Data from ARC and send it to MEGA_FIRMWARE.
3. Receive STOP from ARC and send it to MEGA_FIRMWARE.
4. Receive Pivot Data from ARC and send it to MEGA_FIRMWARE.
5. Receive STOP from MEGA_FIRMWARE and send it to ARC.
6. Send obstacle scan command to ARC when received from MEGA_FIRMWARE.
7. Receive desired bearing periodic updates and send it to ARC.

Notice that MEGA_MGR does not compute or monitor anything, it just provides a UART bridge from MEGA_FIRMWARE to ARC.

So, based upon the GOALS for MEGA_FIRMWARE_v5, and the GOALS for MEGA_MGR_v5, can you please reexamine them for compliance and rewrite as necessary?

Attached are Teensy (IR_TRACKERv4.ino.h.cpp and PacketHandler.h.cpp) for your consideration.

Thank you. Jack

TEENSY:

// IR_TRACKERv4.ino

#include "IR_TRACKERv4.h"
void setup() {  setupTracker();}
void loop() {  loopTracker();}

//IR_TRACKERv4.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 = 60; //15static 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.cpp

#include "IR_TRACKERv4.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 = roundedBearing - RUNAWAY_MARGIN;      //0;int posAbortLimit = roundedBearing + RUNAWAY_MARGIN;     //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);
Serial.print("irNorm:  ");Serial.println(irNorm);Serial.print("negInit:  ");Serial.println(current);Serial.print("negAbortLimit:  ");Serial.println(negAbortLimit);
  if (current < negAbortLimit) {    handleRunaway();    return;  }
  if (reflectorDetected) {    negInit = current;    updateBearingFromInitEdges();    state = NEG_SEARCH_FINAL;    runawayCount = 0;  } 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);
  Serial.print("irNorm:  ");Serial.println(irNorm);Serial.print("negFinal:  ");Serial.println(current);
  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);
Serial.print("irNorm:  ");Serial.println(irNorm);Serial.print("posInit:  ");Serial.println(current);

  if (current > posAbortLimit) {    handleRunaway();    return;  }
  if (reflectorDetected) {    posInit = current;    updateBearingFromInitEdges();    state = POS_SEARCH_FINAL;    runawayCount = 0;  } 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);
Serial.print("irNorm:  ");Serial.println(irNorm);Serial.print("posFinal:  ");Serial.println(current);
  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();}

//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 >= static_cast<int>(packet.length())) 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";}