Yes - that’s a very useful test result. Since UART_v3 disconnects even after only one pass through the loop, the problem is not the RX parser. It’s much more likely the script is hitting a Jint/ARC runtime issue in the loop body, or the UART skill call is failing in a way that drops the connection.

Let’s isolate it with a transmit-only UART_v3a.

UART_v3a: TX only, no RX

This version:

• sends Waypoint Data once
• can send Stop Motors and Actual Bearing on demand
• has no receive logic
• has a very simple loop
• uses sleep(100) to reduce load

var UART_INDEX = 0;
var UART_HEADER = 0xA5;

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

// -----------------------------
// Helpers
// -----------------------------
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 sendPacket(cmd, payload) {
  var packet = [UART_HEADER, cmd].concat(payload);
  UART.hardwareUartWrite(UART_INDEX, packet);
}

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

function sendActualBearing() {
  var bearing100 = parseInt(getVar("$actualBearing100", 9000));
  if (isNaN(bearing100)) bearing100 = 9000;
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = parseInt(getVar("$pathTotalDistance", 0));
  var combinedStepsDesired = parseInt(getVar("$combinedStepsDesired", 0));
  var oppositeDistance = parseInt(getVar("$oppositeDistance", 0));
  var bearingAlignment = parseInt(getVar("$bearingAlignment", 0));
  var desiredBearing100 = parseInt(getVar("$desiredBearing100", 9000));
  var trackDir = parseInt(getVar("$trackDir", 0));

  if (isNaN(pathTotalDistance)) pathTotalDistance = 0;
  if (isNaN(combinedStepsDesired)) combinedStepsDesired = 0;
  if (isNaN(oppositeDistance)) oppositeDistance = 0;
  if (isNaN(bearingAlignment)) bearingAlignment = 0;
  if (isNaN(desiredBearing100)) desiredBearing100 = 9000;
  if (isNaN(trackDir)) trackDir = 0;

  // Payload:
  // 0-3  pathTotalDistance
  // 4-7  combinedStepsDesired
  // 8-11 oppositeDistance
  // 12   bearingAlignment
  // 13-14 desiredBearing100
  // 15-16 reserved
  // 17   trackDir
  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment & 0xFF);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload.push(0x00);
  payload.push(0x00);
  payload.push(trackDir & 0xFF);

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

// -----------------------------
// Main
// -----------------------------
print("UART_v3a started");

var waypointSent = false;

while (true) {
  if (getVar("$sendWaypointNow", 0) == 1 && waypointSent == false) {
    print("$sendWaypointNow = 1, sending waypoint");
    sendWaypointData();
    setVar("$sendWaypointNow", 0);
    waypointSent = true;
    print("$sendWaypointNow: 0");
  }

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

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

  print("loop");
  sleep(100);
}

Why this is the right next test

This removes:

• RX parsing
• packet receive state issues
• extra variables like $run, $obsScan, $legComplete
• any chance the crash is coming from processing incoming data

If this still disconnects, then the issue is almost certainly one of these:

1. UART hardware skill naming / port configuration
2. UART.hardwareUartWrite() behavior in your ARC version
3. A project conflict because the UART skill is being started without the expected project state
4. A malformed global variable or another skill reacting to the transmit

Test instructions

Please run UART_v3a by itself and tell me exactly:

• Does it still disconnect?
• Does it print TX: WAYPOINT DATA sent?
• Does it continue printing loop more than once?
• Does it crash only after sendWaypointData() or even before that?

One important question

Which UART skill are you using in ARC?

• Hardware UART
• Serial Terminal
• another plugin UART skill

That matters because the command syntax can differ slightly depending on the skill.

If you want, after this test I can give you a UART_v3b transmit-only version that avoids UART.hardwareUartWrite() entirely and uses a different ARC method, just to isolate whether the write call itself is the trigger.

@Athena. Boy, do I feel stupid. And you prompted me to check it when you sent:

One important question

Which UART skill are you using in ARC?

• Hardware UART
• Serial Terminal
• another plugin UART skill

Guess what I was missing all this time?

UART.initHardwareUart(0, 115200);

I placed this at the top of the ROVER_CMD_v1 script, and everything seems to work just fine now. My other scripts had 'UART.init', but they are now long gone. I simply can't believe I overlooked something this elementary. I will do in-depth checking during the next week as I'm sure to have a few bugs yet. Can you please do a final write, let's call it v4, of ROVER_CMD, LEG_, and UART_? Also optimize Arduino to match ARC. Let's keep my oversight on the lowdown. Jack

Great catch - that missing UART.initHardwareUart(0, 115200); was the key. No need to dwell on it; the important part is that you found it and the system is moving again.

Below is a clean v4 package for:

• ROVER_CMD_v4
• LEG_v4
• UART_v4
• a matching Arduino sketch update

I’m also simplifying a few things so the ARC side and Arduino side stay aligned and easier to debug.

1) ROVER_CMD_v4

This is the startup coordinator. It initializes UART, starts the leg script, and starts the UART script.

print("ROVER_CMD_v4 started");

UART.initHardwareUart(0, 115200);

// Start Leg first so globals are ready
controlCommand("LEG_v4", "ScriptStartWait");

// Start UART after globals are loaded
controlCommand("UART_v4", "ScriptStart");

print("ROVER_CMD_v4 complete");

2) LEG_v4

This sets the navigation globals that UART_v4 will transmit to the Arduino.

This version keeps the payload aligned with the 18-byte waypoint format we standardized on.

print("LEG_v4 started");

// Example values - replace with your real leg data
setVar("$pathTotalDistance", 50000);
setVar("$combinedStepsDesired", 50000);
setVar("$oppositeDistance", 12000);

// 0 = DA, 1 = OA
setVar("$bearingAlignment", 1);

// desired bearing x100
setVar("$desiredBearing100", 8300);

// current actual bearing x100
setVar("$actualBearing100", 8300);

// 0 = forward, 1 = reverse
setVar("$trackDir", 0);

// State flags
setVar("$sendWaypointNow", 1);
setVar("$sendStopNow", 0);
setVar("$sendActualBearingNow", 0);
setVar("$obsScan", 0);
setVar("$run", 0);
setVar("$legComplete", 0);

print("LEG_v4 complete");

3) UART_v4

This version is cleaned up to:

• initialize safely
• send waypoint once
• send actual bearing on demand
• send stop on demand
• process Arduino responses
• avoid the earlier loop instability

var UART_INDEX = 0;
var UART_HEADER = 0xA5;

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

// Arduino -> ARC
var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;

// ----------------------------------------------------
// Helpers
// ----------------------------------------------------
function u16ToBytes(value) {
  value = value & 0xFFFF;
  return [
    value & 0xFF,
    (value >> 8) & 0xFF
  ];
}

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

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

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

function sendPacket(cmd, payload) {
  var packet = [UART_HEADER, cmd].concat(payload);
  UART.hardwareUartWrite(UART_INDEX, packet);
}

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

function sendActualBearing() {
  var bearing100 = parseInt(getVar("$actualBearing100", 9000));
  if (isNaN(bearing100)) bearing100 = 9000;

  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = parseInt(getVar("$pathTotalDistance", 0));
  var combinedStepsDesired = parseInt(getVar("$combinedStepsDesired", 0));
  var oppositeDistance = parseInt(getVar("$oppositeDistance", 0));
  var bearingAlignment = parseInt(getVar("$bearingAlignment", 0));
  var desiredBearing100 = parseInt(getVar("$desiredBearing100", 9000));
  var trackDir = parseInt(getVar("$trackDir", 0));

  if (isNaN(pathTotalDistance)) pathTotalDistance = 0;
  if (isNaN(combinedStepsDesired)) combinedStepsDesired = 0;
  if (isNaN(oppositeDistance)) oppositeDistance = 0;
  if (isNaN(bearingAlignment)) bearingAlignment = 0;
  if (isNaN(desiredBearing100)) desiredBearing100 = 9000;
  if (isNaN(trackDir)) trackDir = 0;

  // 18-byte payload:
  // 0-3  pathTotalDistance
  // 4-7  combinedStepsDesired
  // 8-11 oppositeDistance
  // 12   bearingAlignment
  // 13-14 desiredBearing100
  // 15-16 reserved
  // 17   trackDir
  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment & 0xFF);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload.push(0x00);
  payload.push(0x00);
  payload.push(trackDir & 0xFF);

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

// ----------------------------------------------------
// RX parser
// ----------------------------------------------------
var rxState = 0; // 0=wait header, 1=wait cmd, 2=wait payload
var rxCmd = 0;
var rxExpected = 0;
var rxPayload = [];
var rxIndex = 0;

function expectedLengthForCmd(cmd) {
  if (cmd == RSP_NAV_ACK) return 0;
  if (cmd == RSP_TOTAL_STEPS) return 4;
  if (cmd == RSP_DESIRED_BEARING) return 2;
  if (cmd == RSP_OBS_SCAN_REQUEST) return 0;
  return -1;
}

function handleRxPacket(cmd, payload) {
  if (cmd == RSP_NAV_ACK) {
    print("RX: NAV ACK");
    setVar("$run", 1);
    return;
  }

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

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

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    print("RX: OBSTACLE SCAN REQUEST");
    setVar("$obsScan", 1);
    return;
  }

  print("RX: Unknown command = 0x" + cmd.toString(16));
}

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

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

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

    if (rxState == 1) {
      rxCmd = b;
      rxExpected = expectedLengthForCmd(rxCmd);
      rxPayload = [];
      rxIndex = 0;

      if (rxExpected == 0) {
        handleRxPacket(rxCmd, rxPayload);
        rxState = 0;
      } else if (rxExpected > 0) {
        rxState = 2;
      } else {
        rxState = 0;
      }
      continue;
    }

    if (rxState == 2) {
      rxPayload.push(b);
      rxIndex++;

      if (rxIndex >= rxExpected) {
        handleRxPacket(rxCmd, rxPayload);
        rxState = 0;
        rxCmd = 0;
        rxExpected = 0;
        rxPayload = [];
        rxIndex = 0;
      }
      continue;
    }
  }
}

// ----------------------------------------------------
// Main loop
// ----------------------------------------------------
print("UART_v4 started");

var waypointSent = false;

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

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

  if (getVar("$sendWaypointNow", 0) == 1 && waypointSent == false) {
    print("$sendWaypointNow = 1, sending waypoint");
    sendWaypointData();
    setVar("$sendWaypointNow", 0);
    waypointSent = true;
  }

  processIncomingBytes();
  sleep(20);
}

4) Arduino optimization to match ARC

Since ARC is now stable, the Arduino should stay simple and predictable. The most important changes are:

• match the 18-byte waypoint payload
• keep NAV ACK, TOTAL STEPS, DESIRED BEARING, OBS SCAN REQUEST
• update actualBearing100 and bearingDiffTarget immediately on CMD_ACTUAL_BEARING
• use neutral correction during scan request, but keep moving
• make pre-run pivots blocking
• use step-pulse totals for distance

If you want, I can provide the final corrected Arduino v4 sketch next in one clean paste, aligned exactly to this ARC v4 package.

5) Recommended startup order

In ARC:

1. ROVER_CMD_v4
2. which runs LEG_v4
3. then starts UART_v4

And make sure UART.initHardwareUart(0, 115200); stays in ROVER_CMD_v4 before the skills start.

6) One question before I write the Arduino v4

Do you want the Arduino v4 to:

• keep the current 18-byte waypoint format, or
• expand back to the original 23-byte format you described earlier?

My recommendation is to keep the 18-byte format, because it is already working with your current ARC flow.

If you want, I can now write the final Arduino v4 to match this exactly.

@Athena. Yes, please write the Arduino_v4 sketch and keep the 18-byte waypoint format. Thank you. Jack

@Athena. BTW did you want to keep 'var waypointSent = false' in the MAIN LOOP of UART_v4? Thanks Jack.

Yes - keep var waypointSent = false; in UART_v4.

It’s useful because it prevents the waypoint packet from being sent repeatedly every loop once $sendWaypointNow is set. With this pattern:

• the packet sends once
• the flag gets cleared
• waypointSent blocks accidental resends

That said, if you later want to send a new waypoint for each leg, you should reset waypointSent = false when a new leg starts, otherwise UART_v4 will only transmit the first waypoint.

Recommended pattern

When a new leg begins in LEG_v4 or ROVER_CMD_v4, set:

setVar("$sendWaypointNow", 1);

And in UART_v4, keep:

var waypointSent = false;

Then if you want to reuse UART for another leg in the same session, reset it at the start of the next leg:

waypointSent = false;

One important note

Because waypointSent is a local script variable, it will reset if the script stops and restarts. So it’s fine to keep as-is for a single run, but if you expect UART_v4 to stay running continuously across multiple legs, I can also show you a cleaner version that uses a global ARC variable like:

setVar("$waypointSent", 0);

That makes it easier to manage across script restarts.

If you want, I can now give you the full Arduino_v4 sketch next, matched to the 18-byte waypoint format and the current UART_v4 behavior.

@Athena. I expect UART_v4 to stay running continuously across multiple legs, please show a cleaner version that uses a global ARC variable like:

setVar("$waypointSent", 0);

That makes it easier to manage across script restarts. Please send a UART_v4 that is a cleaner version using an ARC global variable. Also please send and Arduino_v4 that will match UART_v4. Thanks. Jack

Absolutely - for a continuous multi-leg run, a global ARC variable is the cleaner choice.

Use:

setVar("$waypointSent", 0);

That way:

• the flag survives script restarts better than a local variable
• you can reset it when starting a new leg
• it stays aligned with your ARC state machine

Below is a cleaned-up UART_v4 and a matching Arduino_v4 sketch using the 18-byte waypoint format.

UART_v4

This version:

• initializes hardware UART safely
• sends waypoint data once per leg using $waypointSent
• handles ARC Arduino packets
• processes Arduino ARC packets
• avoids the earlier disconnect issue by keeping the loop simple

var UART_INDEX = 0;
var UART_HEADER = 0xA5;

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

// Arduino -> ARC
var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;

// ----------------------------------------------------
// Helpers
// ----------------------------------------------------
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 packet = [UART_HEADER, cmd].concat(payload);
  UART.hardwareUartWrite(UART_INDEX, packet);
}

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

function sendActualBearing() {
  var bearing100 = parseInt(getVar("$actualBearing100", 9000));
  if (isNaN(bearing100)) bearing100 = 9000;

  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = parseInt(getVar("$pathTotalDistance", 0));
  var combinedStepsDesired = parseInt(getVar("$combinedStepsDesired", 0));
  var oppositeDistance = parseInt(getVar("$oppositeDistance", 0));
  var bearingAlignment = parseInt(getVar("$bearingAlignment", 0));
  var desiredBearing100 = parseInt(getVar("$desiredBearing100", 9000));
  var trackDir = parseInt(getVar("$trackDir", 0));

  if (isNaN(pathTotalDistance)) pathTotalDistance = 0;
  if (isNaN(combinedStepsDesired)) combinedStepsDesired = 0;
  if (isNaN(oppositeDistance)) oppositeDistance = 0;
  if (isNaN(bearingAlignment)) bearingAlignment = 0;
  if (isNaN(desiredBearing100)) desiredBearing100 = 9000;
  if (isNaN(trackDir)) trackDir = 0;

  // 18-byte payload:
  // 0-3  pathTotalDistance      uint32
  // 4-7  combinedStepsDesired   uint32
  // 8-11 oppositeDistance       uint32
  // 12   bearingAlignment       uint8
  // 13-14 desiredBearing100     uint16
  // 15-16 actualBearing100      uint16
  // 17   trackDir               uint8
  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment & 0xFF);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload = payload.concat(u16ToBytes(parseInt(getVar("$actualBearing100", 9000))));
  payload.push(trackDir & 0xFF);

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

// ----------------------------------------------------
// RX handling
// ----------------------------------------------------
var rxState = 0; // 0=wait header, 1=cmd, 2=payload
var rxCmd = 0;
var rxExpected = 0;
var rxPayload = [];
var rxIndex = 0;

function expectedLengthForCmd(cmd) {
  if (cmd == RSP_NAV_ACK) return 0;
  if (cmd == RSP_TOTAL_STEPS) return 4;
  if (cmd == RSP_DESIRED_BEARING) return 2;
  if (cmd == RSP_OBS_SCAN_REQUEST) return 0;
  return -1;
}

function handleRxPacket(cmd, payload) {
  if (cmd == RSP_NAV_ACK) {
    print("RX: NAV ACK");
    setVar("$run", 1);
    return;
  }

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

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

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    print("RX: OBSTACLE SCAN REQUEST");
    setVar("$obsScan", 1);
    return;
  }

  print("RX: Unknown command = 0x" + cmd.toString(16));
}

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

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

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

    if (rxState == 1) {
      rxCmd = b;
      rxExpected = expectedLengthForCmd(rxCmd);
      rxPayload = [];
      rxIndex = 0;

      if (rxExpected == 0) {
        handleRxPacket(rxCmd, rxPayload);
        rxState = 0;
      } else if (rxExpected > 0) {
        rxState = 2;
      } else {
        rxState = 0;
      }
      continue;
    }

    if (rxState == 2) {
      rxPayload.push(b);
      rxIndex++;

      if (rxIndex >= rxExpected) {
        handleRxPacket(rxCmd, rxPayload);
        rxState = 0;
        rxCmd = 0;
        rxExpected = 0;
        rxPayload = [];
        rxIndex = 0;
      }
      continue;
    }
  }
}

// ----------------------------------------------------
// Main
// ----------------------------------------------------
print("UART_v4 started");

if (getVar("$waypointSent", 0) == "") {
  setVar("$waypointSent", 0);
}

while (true) {
  if (getVar("$sendWaypointNow", 0) == 1 && getVar("$waypointSent", 0) == 0) {
    print("$sendWaypointNow = 1, sending waypoint");
    sendWaypointData();
    setVar("$sendWaypointNow", 0);
    setVar("$waypointSent", 1);
    print("$waypointSent = 1");
  }

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

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

  processIncomingBytes();
  sleep(20);
}

Arduino_v4

This version matches the UART_v4 packet layout and behavior.

It includes:

• correct UART parsing
• 18-byte waypoint payload
• actual bearing update handling
• desired bearing change threshold of 100 = 1.00 degree
• step-based progress tracking
• obstacle scan request behavior
• neutral steering during scan request while motion continues
• blocking pivots in pre-run
• stepper control with your DRV8825 setup

#include <AccelStepper.h>
#include <MultiStepper.h>

// -------------------- UART PACKETS --------------------
static const uint8_t UART_HEADER = 0xA5;

// ARC -> Arduino
static const uint8_t CMD_STOP_MOTORS = 0x01;
static const uint8_t CMD_ACTUAL_BEARING = 0x03;
static const uint8_t CMD_RIGHT_PIVOT = 0x04;
static const uint8_t CMD_LEFT_PIVOT = 0x05;
static const uint8_t CMD_WAYPOINT_DATA = 0x06;

// Arduino -> ARC
static const uint8_t RSP_NAV_ACK = 0x10;
static const uint8_t RSP_TOTAL_STEPS = 0x11;
static const uint8_t RSP_DESIRED_BEARING = 0x12;
static const uint8_t RSP_OBS_SCAN_REQUEST = 0x13;

// -------------------- STEPPERS --------------------
// DRV8825 STEP/DIR
AccelStepper LeftFrontWheel(AccelStepper::DRIVER, 3, 6);
AccelStepper LeftBackWheel(AccelStepper::DRIVER, 2, 5);
AccelStepper RightFrontWheel(AccelStepper::DRIVER, 12, 13);
AccelStepper RightBackWheel(AccelStepper::DRIVER, 4, 7);

MultiStepper multiStepper;

// -------------------- PARSER --------------------
enum ParserState { WAIT_HEADER, WAIT_CMD, WAIT_PAYLOAD };
ParserState state = WAIT_HEADER;

uint8_t currentCmd = 0;
uint8_t payload[32];
uint8_t payloadIndex = 0;
uint8_t expectedLength = 0;

// -------------------- NAV STATE --------------------
int RUN = 0; // 0 = pre-run, 1 = forward run

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
uint32_t oppositeDistance = 0;

uint8_t bearingAlignment = 0; // 0 = DA, 1 = OA
uint16_t reflectorBearing100 = 0;
uint8_t sensorPrime = 0;
uint16_t waypointDesiredBearing100 = 9000;
uint16_t waypointActualBearing100 = 9000;
uint8_t trackDir = 0; // 0 = FD, 1 = RD

uint16_t actualBearing100 = 9000;
uint16_t desiredBearing100 = 9000;
uint16_t lastSentDesiredBearing100 = 0xFFFF;

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

bool obstacleScanRequested = false;
uint32_t lastObstacleScanStep = 0;

bool scanNeutralMode = false;
uint16_t scanNeutralSpeed = 400;
uint16_t normalSpeed = 800;
uint16_t currentSpeed = 0;
uint16_t rampIncrement = 10;

// If you want more/less frequent scan requests, adjust this.
// 24 inches * 437.5 steps/inch = 10500 steps
const uint32_t scanIntervalSteps = 10500;

// -------------------- BYTE HELPERS --------------------
uint16_t readUInt16LE(const uint8_t* data) {
  return (uint16_t)data[0] | ((uint16_t)data[1] << 8);
}

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

void writeUInt16LE(uint16_t value) {
  Serial3.write((uint8_t)(value & 0xFF));
  Serial3.write((uint8_t)((value >> 8) & 0xFF));
}

void writeUInt32LE(uint32_t value) {
  Serial3.write((uint8_t)(value & 0xFF));
  Serial3.write((uint8_t)((value >> 8) & 0xFF));
  Serial3.write((uint8_t)((value >> 16) & 0xFF));
  Serial3.write((uint8_t)((value >> 24) & 0xFF));
}

void sendHeaderAndCmd(uint8_t cmd) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
}

void sendNavAck() {
  sendHeaderAndCmd(RSP_NAV_ACK);
  Serial.println("TX: NAV ACK");
}

void sendTotalSteps(uint32_t steps) {
  sendHeaderAndCmd(RSP_TOTAL_STEPS);
  writeUInt32LE(steps);
  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  sendHeaderAndCmd(RSP_DESIRED_BEARING);
  writeUInt16LE(bearing100);
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
  lastSentDesiredBearing100 = bearing100;
}

void sendObstacleScanRequest() {
  sendHeaderAndCmd(RSP_OBS_SCAN_REQUEST);
  Serial.println("TX: OBSTACLE SCAN REQUEST");
}

// -------------------- MOTOR HELPERS --------------------
void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  LeftFrontWheel.setSpeed(leftSpeed);
  LeftBackWheel.setSpeed(leftSpeed);
  RightFrontWheel.setSpeed(rightSpeed);
  RightBackWheel.setSpeed(rightSpeed);
}

void stopMotors() {
  LeftFrontWheel.stop();
  LeftBackWheel.stop();
  RightFrontWheel.stop();
  RightBackWheel.stop();

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

  Serial.println("Action: stopMotors()");
}

void pivotRight(uint16_t steps) {
  Serial.print("Action: pivotRight(");
  Serial.print(steps);
  Serial.println(")");

  long positions[4];
  positions[0] = LeftFrontWheel.currentPosition() + steps;
  positions[1] = LeftBackWheel.currentPosition() + steps;
  positions[2] = RightFrontWheel.currentPosition() - steps;
  positions[3] = RightBackWheel.currentPosition() - steps;

  multiStepper.moveTo(positions);
  while (multiStepper.run()) {
    // blocking pivot
  }

  totalStepsTaken += steps;
  stopMotors();
  sendTotalSteps(totalStepsTaken);
}

void pivotLeft(uint16_t steps) {
  Serial.print("Action: pivotLeft(");
  Serial.print(steps);
  Serial.println(")");

  long positions[4];
  positions[0] = LeftFrontWheel.currentPosition() - steps;
  positions[1] = LeftBackWheel.currentPosition() - steps;
  positions[2] = RightFrontWheel.currentPosition() + steps;
  positions[3] = RightBackWheel.currentPosition() + steps;

  multiStepper.moveTo(positions);
  while (multiStepper.run()) {
    // blocking pivot
  }

  totalStepsTaken += steps;
  stopMotors();
  sendTotalSteps(totalStepsTaken);
}

void beginForwardNavigation() {
  Serial.println("Action: beginForwardNavigation()");
  RUN = 1;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  scanNeutralMode = false;
  obstacleScanRequested = false;
  lastObstacleScanStep = totalStepsTaken;
}

// -------------------- BEARING MATH --------------------
void computeDesiredBearing() {
  if (bearingAlignment == 0) {
    desiredBearing100 = 9000;
    return;
  }

  if (combinedStepsDesired == 0) return;

  uint32_t pathDistanceRemaining = 0;

  if (trackDir == 0) {
    if (pathTotalDistance > totalStepsTaken) {
      pathDistanceRemaining = pathTotalDistance - totalStepsTaken;
    } else {
      pathDistanceRemaining = 1;
    }
  } else {
    if (pathTotalDistance > (combinedStepsDesired - totalStepsTaken)) {
      pathDistanceRemaining = pathTotalDistance - (combinedStepsDesired - totalStepsTaken);
    } else {
      pathDistanceRemaining = 1;
    }
  }

  if (pathDistanceRemaining == 0) pathDistanceRemaining = 1;

  float tangent = (float)oppositeDistance / (float)pathDistanceRemaining;
  float deg = atan(tangent) * 180.0 / PI;

  desiredBearing100 = (uint16_t)((deg + 90.0) * 100.0);

  if (abs((int)desiredBearing100 - (int)lastSentDesiredBearing100) >= 100) {
    sendDesiredBearing(desiredBearing100);
  }
}

void updateBearingDiff() {
  bearingDiffTarget = (int16_t)actualBearing100 - (int16_t)desiredBearing100;

  if (bearingDiffRamped < bearingDiffTarget) {
    bearingDiffRamped += 10;
    if (bearingDiffRamped > bearingDiffTarget) bearingDiffRamped = bearingDiffTarget;
  } else if (bearingDiffRamped > bearingDiffTarget) {
    bearingDiffRamped -= 10;
    if (bearingDiffRamped < bearingDiffTarget) bearingDiffRamped = bearingDiffTarget;
  }
}

// -------------------- WPT HANDLING --------------------
void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) {
    Serial.print("Waypoint length mismatch: ");
    Serial.println(len);
    return;
  }

  pathTotalDistance = readUInt32LE(&data[0]);
  combinedStepsDesired = readUInt32LE(&data[4]);
  oppositeDistance = readUInt32LE(&data[8]);
  bearingAlignment = data[12];
  waypointDesiredBearing100 = readUInt16LE(&data[13]);
  waypointActualBearing100 = readUInt16LE(&data[15]);
  trackDir = data[17];

  desiredBearing100 = waypointDesiredBearing100;
  actualBearing100 = waypointActualBearing100;
  lastSentDesiredBearing100 = 0xFFFF;

  Serial.println("RX: WAYPOINT DATA received");
  Serial.print("pathTotalDistance = "); Serial.println(pathTotalDistance);
  Serial.print("combinedStepsDesired = "); Serial.println(combinedStepsDesired);
  Serial.print("oppositeDistance = "); Serial.println(oppositeDistance);
  Serial.print("bearingAlignment = "); Serial.println(bearingAlignment);
  Serial.print("desiredBearing100 = "); Serial.println(desiredBearing100);
  Serial.print("actualBearing100 = "); Serial.println(actualBearing100);
  Serial.print("trackDir = "); Serial.println(trackDir);

  sendNavAck();
  beginForwardNavigation();
}

// -------------------- PACKET HANDLING --------------------
uint8_t getExpectedLength(uint8_t cmd) {
  switch (cmd) {
    case CMD_STOP_MOTORS: return 0;
    case CMD_ACTUAL_BEARING: return 2;
    case CMD_RIGHT_PIVOT: return 2;
    case CMD_LEFT_PIVOT: return 2;
    case CMD_WAYPOINT_DATA: return 18;
    default: return 0xFF;
  }
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      Serial.println("RX: STOP MOTORS");
      stopMotors();
      RUN = 0;
      sendTotalSteps(totalStepsTaken);
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("RX: ACTUAL BEARING x100 = ");
        Serial.println(actualBearing100);

        updateBearingDiff();
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2 && RUN == 0) {
        pivotRight(readUInt16LE(data));
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && RUN == 0) {
        pivotLeft(readUInt16LE(data));
      }
      break;

    case CMD_WAYPOINT_DATA:
      if (RUN == 0) {
        handleWaypointPacket(data, len);
      } else {
        Serial.println("RX: WAYPOINT ignored because RUN != 0");
      }
      break;
  }
}

void processIncomingByte(uint8_t b) {
  switch (state) {
    case WAIT_HEADER:
      if (b == UART_HEADER) state = WAIT_CMD;
      break;

    case WAIT_CMD:
      currentCmd = b;
      expectedLength = getExpectedLength(currentCmd);

      if (expectedLength == 0xFF) {
        state = WAIT_HEADER;
      } else if (expectedLength == 0) {
        handlePacket(currentCmd, nullptr, 0);
        state = WAIT_HEADER;
      } else {
        payloadIndex = 0;
        state = WAIT_PAYLOAD;
      }
      break;

    case WAIT_PAYLOAD:
      payload[payloadIndex++] = b;

      if (payloadIndex >= expectedLength) {
        handlePacket(currentCmd, payload, expectedLength);
        state = WAIT_HEADER;
      }
      break;
  }
}

void readIncomingUart() {
  while (Serial3.available() > 0) {
    uint8_t b = (uint8_t)Serial3.read();
    processIncomingByte(b);
  }
}

// -------------------- SETUP / LOOP --------------------
void setup() {
  Serial.begin(115200);
  Serial3.begin(115200);

  LeftFrontWheel.setMaxSpeed(2000);
  LeftBackWheel.setMaxSpeed(2000);
  RightFrontWheel.setMaxSpeed(2000);
  RightBackWheel.setMaxSpeed(2000);

  LeftFrontWheel.setAcceleration(800);
  LeftBackWheel.setAcceleration(800);
  RightFrontWheel.setAcceleration(800);
  RightBackWheel.setAcceleration(800);

  multiStepper.addStepper(LeftFrontWheel);
  multiStepper.addStepper(LeftBackWheel);
  multiStepper.addStepper(RightFrontWheel);
  multiStepper.addStepper(RightBackWheel);

  Serial.println("Arduino_v4 ready");
}

void loop() {
  readIncomingUart();

  if (RUN == 0) {
    // pre-run mode: wait for pivots and waypoint
  } else if (RUN == 1) {
    // ramp up speed
    if (currentSpeed < normalSpeed && !scanNeutralMode) {
      currentSpeed += rampIncrement;
      if (currentSpeed > normalSpeed) currentSpeed = normalSpeed;
    }

    // compute and send desired bearing if needed
    computeDesiredBearing();

    // ramp bearing correction
    updateBearingDiff();

    // If scan request is active, keep rover moving but neutralize differential
    int leftSpeed = currentSpeed;
    int rightSpeed = currentSpeed;

    if (scanNeutralMode) {
      leftSpeed = currentSpeed;
      rightSpeed = currentSpeed;
    } else {
      int correction = bearingDiffRamped / 10;
      leftSpeed -= correction;
      rightSpeed += correction;
    }

    setAllMotorSpeeds(leftSpeed, rightSpeed);

    // Run steppers
    LeftFrontWheel.runSpeed();
    LeftBackWheel.runSpeed();
    RightFrontWheel.runSpeed();
    RightBackWheel.runSpeed();

    // Use accumulated step targets
    totalStepsTaken =
      abs(LeftFrontWheel.currentPosition()) +
      abs(LeftBackWheel.currentPosition()) +
      abs(RightFrontWheel.currentPosition()) +
      abs(RightBackWheel.currentPosition());

    // Scan request about every 24 inches
    if (!obstacleScanRequested &&
        totalStepsTaken > 0 &&
        (totalStepsTaken - lastObstacleScanStep) >= scanIntervalSteps) {

      sendObstacleScanRequest();
      obstacleScanRequested = true;
      scanNeutralMode = true;
      lastObstacleScanStep = totalStepsTaken;
      Serial.println("Obstacle scan active, steering set neutral");
    }

    // Leg complete
    if (totalStepsTaken >= combinedStepsDesired && combinedStepsDesired > 0) {
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      RUN = 0;
      Serial.println("Leg complete");
    }
  }
}

Notes

About $waypointSent

Yes, in this global version, you should initialize it in LEG_v4 or ROVER_CMD_v4:

setVar("$waypointSent", 0);

Then when a new leg starts, reset it again:

setVar("$waypointSent", 0);
setVar("$sendWaypointNow", 1);

About the 18-byte waypoint format

This version uses:

• pathTotalDistance = 4 bytes
• combinedStepsDesired = 4 bytes
• oppositeDistance = 4 bytes
• bearingAlignment = 1 byte
• desiredBearing100 = 2 bytes
• actualBearing100 = 2 bytes
• trackDir = 1 byte

Total = 18 bytes

One thing I want to confirm

For your actual production flow, do you want the Arduino to:

1. continue moving during scan request with neutral steering, exactly as above, or
2. pause step speed briefly during the scan request and then resume?

I assumed continue moving, since that matches what you described.

If you want, I can next give you:

• a cleaned up ROVER_CMD_v4
• a slightly more defensive LEG_v4
• or a UART_v4b with extra debug prints for first-run validation