Left Motors Stall Until Scan Request

Yes - the behavior is coming from the sketch logic, not ARC itself.

Short answer

Your left motors are being starved until sendObstacleScanRequest() runs because runForwardLoop() is the only place where motor speeds are continuously updated and runSpeed() is called.
Until that loop is actively running, the stepper drivers won’t be serviced often enough for AccelStepper to generate motion.

But there’s a second, more important issue:

Your code only enters forward-running mode after the waypoint packet is handled

beginForwardNavigation() sets:

• RUN = 1
• resets currentSpeed
• resets scan flags

So the real question is: why do the left motors appear dead until the first obstacle scan request?

That points to the speed-correction logic and the scan-neutral behavior.

What I see in the sketch

1) Motor motion only happens inside runForwardLoop()

This block is critical:

if (RUN == 1) {
  runForwardLoop();
}

And inside runForwardLoop() you do all of this:

• compute bearing
• update speed
• apply left/right correction
• call setAllMotorSpeeds(leftSpeed, rightSpeed);
• call runSpeed() on all 4 motors

So if RUN is not 1, nothing moves.

2) Your left motors are probably being driven to 0 or negative speed before the scan request

This section is suspicious:

int correction = bearingDiffRamped / 10;
leftSpeed -= correction;
rightSpeed += correction;
setAllMotorSpeeds(leftSpeed, rightSpeed);

If bearingDiffRamped is positive enough, the correction can push leftSpeed down to zero or below.

Because you’re using:

LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();

runSpeed() does not accelerate or recover a stalled motor if the speed is 0. It only steps when the speed is set properly.

So the left side may look "stalled" while the right side continues or the system waits for the scan event.

3) scanNeutralMode changes the correction behavior

Once this happens:

sendObstacleScanRequest();
scanNeutralMode = true;

then this block no longer applies correction:

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

That means both sides go back to the same currentSpeed, which makes the left motors suddenly start moving.

So the symptom fits this pattern:

• before scan request: correction reduces left speed too much
• after scan request: correction is disabled
• left motors move again

There is also a bug in your step averaging line

This line has operator precedence trouble:

totalStepsTaken = abs(LeftFrontWheel.currentPosition()) + abs(LeftBackWheel.currentPosition()) + abs(RightFrontWheel.currentPosition()) + abs(RightBackWheel.currentPosition()) / 4;

Only the last term is divided by 4.

It should probably be:

totalStepsTaken =
  (abs(LeftFrontWheel.currentPosition()) +
   abs(LeftBackWheel.currentPosition()) +
   abs(RightFrontWheel.currentPosition()) +
   abs(RightBackWheel.currentPosition())) / 4;

If ARC is using totalStepsTaken to decide when scans happen, this bug could make scan timing very wrong.

Another issue: currentSpeed may never ramp up the way you expect

You do:

if (currentSpeed < normalSpeed) currentSpeed += 10;
if (currentSpeed > normalSpeed) currentSpeed = normalSpeed;

But I don’t see the full runForwardLoop() definition here, so if there’s any early return, or if currentSpeed is reset unexpectedly, the motors may never get enough speed to overcome static friction.

That often shows up more on one side than the other.

Most likely root cause

The most likely cause is:

Your left motors are being reduced to 0 or near-0 by the bearing correction logic until scanNeutralMode is enabled by sendObstacleScanRequest().

So the obstacle scan request is not fixing the motors - it’s disabling the correction that was suppressing the left side.

What I recommend checking first

Add debug prints inside the loop right before setAllMotorSpeeds():

Serial.print("RUN=");
Serial.print(RUN);
Serial.print(" currentSpeed=");
Serial.print(currentSpeed);
Serial.print(" bearingDiffRamped=");
Serial.print(bearingDiffRamped);
Serial.print(" leftSpeed=");
Serial.print(leftSpeed);
Serial.print(" rightSpeed=");
Serial.print(rightSpeed);
Serial.print(" scanNeutralMode=");
Serial.println(scanNeutralMode);

If you see leftSpeed <= 0 before the scan request, that confirms it.

Safer fix

Clamp the speeds before sending them to the steppers:

leftSpeed = constrain(leftSpeed, 0, normalSpeed);
rightSpeed = constrain(rightSpeed, 0, normalSpeed);
setAllMotorSpeeds(leftSpeed, rightSpeed);

If you need reverse capability, handle negative values intentionally instead of feeding them directly to runSpeed().

One more thing to verify

Make sure your runForwardLoop() is actually being called continuously and not only after a scan event. If you can paste that function, I can review it line by line - that’s probably where the exact left-side stall is happening.

Relevant ARC note

Since this is acting like a hardware controller for ARC, your setup fits the kind of microcontroller offload ARC is designed for: https://synthiam.com/Support/ARC-Overview/arc-and-micros-for-closed-loop-functions

If you want, I can also help you:

• rewrite this into cleaner Arduino/ARC UART protocol code
• add speed clamping and motor debug telemetry
• or convert the logic into a more robust state machine for waypoint + scan handling

If you paste runForwardLoop() and the exact serial packet flow from ARC, I’ll pinpoint the fault more precisely.

@Athena. Can you please rewrite this into cleaner Arduino/ARC UART protocol code?

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

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;

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;

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;

int RUN = 0;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
uint32_t oppositeDistance = 0;
uint8_t bearingAlignment = 0;
uint16_t reflectorBearing100 = 0;
uint8_t sensorPrime = 0;
uint16_t waypointDesiredBearing100 = 9000;
uint16_t waypointActualBearing100 = 9000;
uint8_t trackDir = 0;

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

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

bool obstacleScanRequested = false;
bool scanNeutralMode = false;

uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

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);
  lastSentDesiredBearing100 = bearing100;
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

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

void resetStepperPositions() {
  LeftFrontWheel.setCurrentPosition(0);
  LeftBackWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);
  RightBackWheel.setCurrentPosition(0);
}

void resetNavigationState() {
  resetStepperPositions();
  totalStepsTaken = 0;
  desiredBearing100 = 9000;
  actualBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  RUN = 0;
  Serial.println("Navigation state reset");
  Serial.print("desiredBearing100:  ");
  Serial.println(desiredBearing100);
  Serial.print("totalStepsTaken:  ");
  Serial.println(totalStepsTaken);
  Serial.print("combinedStepsDesired:  ");
  Serial.println(combinedStepsDesired);
  Serial.print("pathTotalDistance:  ");
  Serial.println(pathTotalDistance);
}

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) {
  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()) { }
  totalStepsTaken += steps;
  stopMotors();
  sendTotalSteps(totalStepsTaken);
  resetNavigationState();
}

void pivotLeft(uint16_t steps) {
  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()) { }
  totalStepsTaken += steps;
  stopMotors();
  sendTotalSteps(totalStepsTaken);
  resetNavigationState();
}

void beginForwardNavigation() {
  RUN = 1;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

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

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

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

void runForwardLoop() {
  if (currentSpeed < normalSpeed) {
    currentSpeed += 10;
    if (currentSpeed > normalSpeed) currentSpeed = normalSpeed;
  }

  computeDesiredBearing();
  updateBearingDiff();

  int leftSpeed = currentSpeed;
  int rightSpeed = currentSpeed;

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

  Serial.print("RUN=");
  Serial.print(RUN);
  Serial.print(" currentSpeed=");
  Serial.print(currentSpeed);
  Serial.print(" bearingDiffRamped=");
  Serial.print(bearingDiffRamped);
  Serial.print(" leftSpeed=");
  Serial.print(leftSpeed);
  Serial.print(" rightSpeed=");
  Serial.print(rightSpeed);
  Serial.print(" scanNeutralMode=");
  Serial.println(scanNeutralMode);

  setAllMotorSpeeds(leftSpeed, rightSpeed);
  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();

  totalStepsTaken =
    abs(LeftFrontWheel.currentPosition()) +
    abs(LeftBackWheel.currentPosition()) +
    abs(RightFrontWheel.currentPosition()) +
    abs(RightBackWheel.currentPosition());

  //if (!obstacleScanRequested && totalStepsTaken >= nextScanStep) {
  //    sendObstacleScanRequest();
  //    obstacleScanRequested = true;
  //    scanNeutralMode = true;
  //    nextScanStep += scanIntervalSteps;
  //}

  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    scanNeutralMode = true;
    nextScanStep += scanIntervalSteps;
  }
  if (totalStepsTaken >= (combinedStepsDesired) && combinedStepsDesired > 0) {
    stopMotors();
    sendTotalSteps(totalStepsTaken);
    resetNavigationState();
  }
}

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 handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) return;

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

  desiredBearing100 = waypointDesiredBearing100;
  actualBearing100 = waypointActualBearing100;

  sendNavAck();
  beginForwardNavigation();
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      resetNavigationState();
      break;
    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("actualBearing100:  ");
        Serial.println(actualBearing100);
      }
      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);
      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) {
    processIncomingByte((uint8_t)Serial3.read());
  }
}

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

  resetNavigationState();
  Serial.println("Arduino_v5 ready");
}

void loop() {
  readIncomingUart();
  if (RUN == 1) {
    runForwardLoop();
  }
}

Absolutely - and I’d strongly recommend cleaning this up into a small framed protocol + a state machine so ARC and the Arduino stay easy to debug.

Below is a much cleaner pattern you can use for ARC Arduino UART with:

• a single packet format
• payload length
• checksum
• separate command handler
• safe speed clamping
• clear navigation state
• and room to expand later

What I changed conceptually

Your current sketch mixes:

• serial parsing
• motor control
• navigation state
• scan scheduling
• bearing math
• ARC responses

That makes bugs hard to isolate.

Cleaner structure

Split into 4 layers:

1. Protocol

   • packet header
   • command
   • length
   • payload
   • checksum

2. Parser

   • reads bytes from Serial3
   • validates packets
   • calls a handler

3. Navigation state

   • stores waypoint data
   • stores scan timing
   • stores bearing corrections

4. Motor service loop

   • computes speeds
   • clamps them
   • drives steppers

Recommended packet format

Use this for all messages:

[0xA5][CMD][LEN][PAYLOAD...][CHECKSUM]

• 0xA5 = header
• CMD = command byte
• LEN = payload length
• PAYLOAD = 0..N bytes
• CHECKSUM = XOR of CMD, LEN, and payload bytes

This is much safer than parsing only by fixed lengths.

Cleaner Arduino sketch template

Here is a rewritten version you can build on.

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

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

static const uint8_t MAX_PAYLOAD = 32;

// =====================
// 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);
MultiStepper multiStepper;

// =====================
// Parser state
// =====================
enum ParserState {
  WAIT_HEADER,
  WAIT_CMD,
  WAIT_LEN,
  WAIT_PAYLOAD,
  WAIT_CHECKSUM
};

ParserState parserState = WAIT_HEADER;
uint8_t currentCmd = 0;
uint8_t expectedLen = 0;
uint8_t payloadIndex = 0;
uint8_t payload[MAX_PAYLOAD];
uint8_t runningChecksum = 0;

// =====================
// Navigation state
// =====================
bool navRunning = false;
bool obstacleScanRequested = false;
bool scanNeutralMode = false;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
uint32_t oppositeDistance = 0;
uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint8_t bearingAlignment = 0;
uint8_t trackDir = 0;

uint16_t reflectorBearing100 = 0;
uint16_t waypointDesiredBearing100 = 9000;
uint16_t waypointActualBearing100 = 9000;
uint16_t actualBearing100 = 9000;
uint16_t desiredBearing100 = 9000;
uint16_t lastSentDesiredBearing100 = 0xFFFF;

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

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

// =====================
// Protocol helpers
// =====================
uint8_t checksumXor(uint8_t cmd, uint8_t len, const uint8_t* data) {
  uint8_t c = cmd ^ len;
  for (uint8_t i = 0; i < len; i++) {
    c ^= data[i];
  }
  return c;
}

void sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
  }
  Serial3.write(checksumXor(cmd, len, data));
}

void sendCmdOnly(uint8_t cmd) {
  sendPacket(cmd, nullptr, 0);
}

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

void sendTotalSteps(uint32_t steps) {
  Serial3.write(UART_HEADER);
  Serial3.write(RSP_TOTAL_STEPS);
  Serial3.write((uint8_t)4);
  writeUInt32LE(steps);

  uint8_t tmp[4];
  tmp[0] = (uint8_t)(steps & 0xFF);
  tmp[1] = (uint8_t)((steps >> 8) & 0xFF);
  tmp[2] = (uint8_t)((steps >> 16) & 0xFF);
  tmp[3] = (uint8_t)((steps >> 24) & 0xFF);
  Serial3.write(checksumXor(RSP_TOTAL_STEPS, 4, tmp));

  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  Serial3.write(UART_HEADER);
  Serial3.write(RSP_DESIRED_BEARING);
  Serial3.write((uint8_t)2);
  writeUInt16LE(bearing100);

  uint8_t tmp[2];
  tmp[0] = (uint8_t)(bearing100 & 0xFF);
  tmp[1] = (uint8_t)((bearing100 >> 8) & 0xFF);
  Serial3.write(checksumXor(RSP_DESIRED_BEARING, 2, tmp));

  lastSentDesiredBearing100 = bearing100;
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

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

// =====================
// Motor helpers
// =====================
void resetStepperPositions() {
  LeftFrontWheel.setCurrentPosition(0);
  LeftBackWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);
  RightBackWheel.setCurrentPosition(0);
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);

  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 resetNavigationState() {
  resetStepperPositions();

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

  bearingDiffTarget = 0;
  bearingDiffRamped = 0;

  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;

  currentSpeed = 0;
  navRunning = false;

  Serial.println("Navigation state reset");
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

// =====================
// Navigation math
// =====================
void computeDesiredBearing() {
  if (bearingAlignment == 0) {
    return;
  }

  uint32_t pathDistanceRemaining = 1;

  if (trackDir == 0) {
    if (pathTotalDistance > totalStepsTaken) {
      pathDistanceRemaining = pathTotalDistance - totalStepsTaken;
    }
  } else {
    uint32_t remainingSteps = (combinedStepsDesired > totalStepsTaken) ? (combinedStepsDesired - totalStepsTaken) : 0;
    if (pathTotalDistance > remainingSteps) {
      pathDistanceRemaining = pathTotalDistance - remainingSteps;
    }
  }

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

void runForwardLoop() {
  if (!navRunning) {
    return;
  }

  if (currentSpeed < normalSpeed) {
    currentSpeed = normalSpeed;
  }

  computeDesiredBearing();
  updateBearingDiff();

  int leftSpeed = currentSpeed;
  int rightSpeed = currentSpeed;

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

  setAllMotorSpeeds(leftSpeed, rightSpeed);

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

  totalStepsTaken =
    (abs(LeftFrontWheel.currentPosition()) +
     abs(LeftBackWheel.currentPosition()) +
     abs(RightFrontWheel.currentPosition()) +
     abs(RightBackWheel.currentPosition())) / 4;

  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    scanNeutralMode = true;
    nextScanStep += scanIntervalSteps;
  }

  if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired) {
    stopMotors();
    sendTotalSteps(totalStepsTaken);
    resetNavigationState();
  }
}

// =====================
// 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 pivotRight(uint16_t steps) {
  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()) { }

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

void pivotLeft(uint16_t steps) {
  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()) { }

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

void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) {
    return;
  }

  pathTotalDistance = readUInt32LE(&data[0]);
  combinedStepsDesired = readUInt32LE(&data[4]);
  oppositeDistance = readUInt32LE(&data[8]);
  bearingAlignment = data[12];
  reflectorBearing100 = readUInt16LE(&data[13]);
  sensorPrime = data[15];
  waypointDesiredBearing100 = readUInt16LE(&data[16]);

  waypointActualBearing100 = actualBearing100;
  desiredBearing100 = waypointDesiredBearing100;

  sendNavAck();
  beginForwardNavigation();
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      resetNavigationState();
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("actualBearing100: ");
        Serial.println(actualBearing100);
      }
      break;

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

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

    case CMD_WAYPOINT_DATA:
      if (!navRunning) {
        handleWaypointPacket(data, len);
      }
      break;
  }
}

// =====================
// UART parser
// =====================
void processIncomingByte(uint8_t b) {
  switch (parserState) {
    case WAIT_HEADER:
      if (b == UART_HEADER) {
        parserState = WAIT_CMD;
        runningChecksum = 0;
      }
      break;

    case WAIT_CMD:
      currentCmd = b;
      runningChecksum ^= b;
      parserState = WAIT_LEN;
      break;

    case WAIT_LEN:
      expectedLen = b;
      runningChecksum ^= b;

      if (expectedLen > MAX_PAYLOAD) {
        parserState = WAIT_HEADER;
      } else if (expectedLen == 0) {
        parserState = WAIT_CHECKSUM;
      } else {
        payloadIndex = 0;
        parserState = WAIT_PAYLOAD;
      }
      break;

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

      if (payloadIndex >= expectedLen) {
        parserState = WAIT_CHECKSUM;
      }
      break;

    case WAIT_CHECKSUM:
      if (b == runningChecksum) {
        handlePacket(currentCmd, payload, expectedLen);
      } else {
        Serial.println("Checksum error");
      }
      parserState = WAIT_HEADER;
      break;
  }
}

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

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

  resetNavigationState();
  Serial.println("Arduino_v5 ready");
}

void loop() {
  readIncomingUart();
  runForwardLoop();
}

Why this version is better

1) Safer protocol

You now have:

• header
• command
• length
• payload
• checksum

That makes bad serial reads much easier to reject.

2) No hidden dependence on scan events

runForwardLoop() runs every loop, not only under a side effect.

3) Speed values are clamped

So the left side can’t get driven into nonsense values.

4) Packet parsing is reusable

You can add more commands later without rewriting the parser.

Important note about your current code

Your original readUInt16LE() is broken in the pasted sketch:

return (uint16_t)data[0] | ((uint16_t)data[1] > 8) & 0xFF));

It should be:

return (uint16_t)data[0] | ((uint16_t)data[1] << 8);

That alone could corrupt bearing values and affect motor correction.

ARC side recommendation

On the ARC side, use the Script skill to send packets with a clean helper function.

If you want, I can also write the ARC JavaScript sender for this protocol so ARC can send:

• CMD_WAYPOINT_DATA
• CMD_ACTUAL_BEARING
• CMD_STOP_MOTORS
• pivot commands

using Serial3/COM with a matching checksum.

ARC docs for scripting:

• https://synthiam.com/Support/Skills/Scripting/Script?id=16089
• JavaScript overview: https://synthiam.com/Docs/javascript-api/javascript-overview

Best next step

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

1. Rewrite the ARC JavaScript sender
2. Add a matching receive parser for ARC
3. Convert this into a cleaner two-file design
   • protocol.h
   • navigation.ino

If you paste the ARC side packet code too, I can make both ends match exactly.

@Athena, Can you please rewrite UART_v5 to match the 'Cleaner Arduino' sketch?. I would also like actualBearing100 and the desiredBearing100 listed in the function [SendWaypointData] to reflect (Servo.getPosition(d17,0) * 100). Thanks. Jack

UART_v5

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;

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

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 = safeInt(getVar("$actualBearing100", 9000), 9000);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0);
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0);
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0);
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0);
//___________________________________________________________________________
  var desiredBearing100 = safeInt(getVar("$desiredBearing100", 9000), 9000);   //(Servo.getPosition(d17,0) * 100)

  var actualBearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);     //(Servo.getPosition(d17,0) * 100)

//____________________________________________________________________________
  var trackDir = safeInt(getVar("$trackDir", 0), 0);

  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(actualBearing100));
  payload.push(trackDir & 0xFF);

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

// RX parser
var rxState = 0;
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);
    setVar("$waypointSent", 0);
    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;
    }
  }
}

print("UART_v5 started");

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

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

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

  processIncomingBytes();
  sleep(20);
}

Absolutely - below is a cleaned-up ARC JavaScript version of UART_v5 that matches the simpler Arduino packet format I suggested:

• 0xA5 header
• CMD
• LEN
• PAYLOAD
• CHECKSUM = XOR of CMD, LEN, and payload bytes

It also updates SendWaypointData() so both actualBearing100 and desiredBearing100 are based on:

Servo.getPosition(d17, 0) * 100

Note: Servo.getPosition() returns the commanded servo position, not the physical position. If d17 is a smart servo and you want its real physical position, we should use Servo.getPositionRealtime() instead.

ARC JavaScript: UART_v5

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;

var rxState = 0; // 0=wait header, 1=cmd, 2=len, 3=payload, 4=checksum
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);
}

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

function sendPacket(cmd, payload) {
  if (payload === undefined || payload === null)
    payload = [];

  var len = payload.length;
  var cs = checksumXor(cmd, len, payload);

  UART.hardwareUartWrite(UART_INDEX, UART_HEADER);
  UART.hardwareUartWrite(UART_INDEX, cmd);
  UART.hardwareUartWrite(UART_INDEX, len);

  for (var i = 0; i < payload.length; i++) {
    UART.hardwareUartWrite(UART_INDEX, payload[i] & 0xFF);
  }

  UART.hardwareUartWrite(UART_INDEX, cs);
}

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

function sendActualBearing() {
  var pos = Servo.getPosition(d17, 0);
  var bearing100 = clamp(pos * 100, 0, 18000);

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

function sendWaypointData() {
  var pos = Servo.getPosition(d17, 0);
  var bearing100 = clamp(pos * 100, 0, 18000);

  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0);
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0);
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0);
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0);
  var reflectorBearing100 = safeInt(getVar("$reflectorBearing100", 9000), 9000);
  var sensorPrime = safeInt(getVar("$sensorPrime", 0), 0);

  // Payload must match Arduino expectation of 18 bytes:
  // [0..3]  pathTotalDistance   u32
  // [4..7]  combinedStepsDesired u32
  // [8..11] oppositeDistance    u32
  // [12]    bearingAlignment    u8
  // [13..14] reflectorBearing100 u16
  // [15]    sensorPrime         u8
  // [16..17] waypointDesiredBearing100 u16
  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(reflectorBearing100));
  payload.push(sensorPrime & 0xFF);
  payload = payload.concat(u16ToBytes(bearing100));

  // Update variables so both reflect Servo.getPosition(d17, 0) * 100
  setVar("$actualBearing100", bearing100);
  setVar("$desiredBearing100", bearing100);

  sendPacket(CMD_WAYPOINT_DATA, payload);
  print("TX: WAYPOINT DATA sent, actual/desired bearing x100 = " + bearing100);
}

function sendRightPivot(steps) {
  steps = safeInt(steps, 0);
  sendPacket(CMD_RIGHT_PIVOT, u16ToBytes(steps));
  print("TX: RIGHT PIVOT = " + steps);
}

function sendLeftPivot(steps) {
  steps = safeInt(steps, 0);
  sendPacket(CMD_LEFT_PIVOT, u16ToBytes(steps));
  print("TX: LEFT PIVOT = " + steps);
}

function expectedLengthForCmd(cmd) {
  switch (cmd) {
    case RSP_NAV_ACK:
      return 0;
    case RSP_TOTAL_STEPS:
      return 4;
    case RSP_DESIRED_BEARING:
      return 2;
    case RSP_OBS_SCAN_REQUEST:
      return 0;
    default:
      return -1;
  }
}

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

  if (cmd == RSP_TOTAL_STEPS) {
    if (payload.length == 4) {
      var steps = bytesToU32LE(payload[0], payload[1], payload[2], payload[3]);
      setVar("$totalStepsTaken", steps);
      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) {
    print("RX: OBSTACLE SCAN REQUEST");
    setVar("$obstacleScanRequest", 1);
    return;
  }

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

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;
      rxChecksum = b;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b & 0xFF;
      rxChecksum ^= b;
      rxPayload = [];
      rxIndex = 0;

      if (rxLen == 0) {
        rxState = 4;
      } else {
        rxState = 3;
      }
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b & 0xFF);
      rxChecksum ^= (b & 0xFF);
      rxIndex++;

      if (rxIndex >= rxLen) {
        rxState = 4;
      }
      continue;
    }

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        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("UART_v5 started");

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

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

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

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

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

  processIncomingBytes();
  sleep(20);
}

Important clarification about d17

You asked for:

Servo.getPosition(d17, 0) * 100

That is valid only if d17 is the servo port constant in ARC.

If you want the physical measured position of a smart servo, use:

Servo.getPositionRealtime(d17, 0) * 100

If you want, I can change the script to use realtime position instead.

One thing to verify

Your Arduino sketch currently expects waypoint payload fields in this exact order and size:

• pathTotalDistance = 4
• combinedStepsDesired = 4
• oppositeDistance = 4
• bearingAlignment = 1
• reflectorBearing100 = 2
• sensorPrime = 1
• waypointDesiredBearing100 = 2

That totals 18 bytes, which is what I matched above.

Suggested ARC variables

This script expects these global variables to exist or be set elsewhere:

• $pathTotalDistance
• $combinedStepsDesired
• $oppositeDistance
• $bearingAlignment
• $reflectorBearing100
• $sensorPrime

And it writes:

• $actualBearing100
• $desiredBearing100
• $navAckReceived
• $totalStepsTaken
• $obstacleScanRequest

Next step

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

1. Rewrite the Arduino sketch to match this exactly
2. Add CRC-8 instead of XOR
3. Remove globals and make SendWaypointData() self-contained
4. Add a debug mode that prints every packet in hex

If you paste your current ARC SendWaypointData() function, I can wire this directly into your existing project naming and variables.

@Athena. The Simpler Arduino sketch does not compile. Under 'void handleWaypointPacket(const uint8_t* data, uint8_t len)' ---'sensorPrime' is flagged as not declared. Arduino Reprinted below. Let's name it ARDUINO_v5 going forward. BTW I do not want sensorPrime sent, only the following:

WAYPOINT DATA

$pathTotalDistance $combinedStepsDesired $oppositeDistance $bearingAlignment $desiredBearing100 $actualBearing100 $trackDir

Can you rewrite ARDUINO_v5? Thanks. Jack

ARDUINO_v5

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

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

static const uint8_t MAX_PAYLOAD = 32;

// =====================
// 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);
MultiStepper multiStepper;

// =====================
// Parser state
// =====================
enum ParserState {
  WAIT_HEADER,
  WAIT_CMD,
  WAIT_LEN,
  WAIT_PAYLOAD,
  WAIT_CHECKSUM
};

ParserState parserState = WAIT_HEADER;
uint8_t currentCmd = 0;
uint8_t expectedLen = 0;
uint8_t payloadIndex = 0;
uint8_t payload[MAX_PAYLOAD];
uint8_t runningChecksum = 0;

// =====================
// Navigation state
// =====================
bool navRunning = false;
bool obstacleScanRequested = false;
bool scanNeutralMode = false;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
uint32_t oppositeDistance = 0;
uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint8_t bearingAlignment = 0;
uint8_t trackDir = 0;

uint16_t reflectorBearing100 = 0;
uint16_t waypointDesiredBearing100 = 9000;
uint16_t waypointActualBearing100 = 9000;
uint16_t actualBearing100 = 9000;
uint16_t desiredBearing100 = 9000;
uint16_t lastSentDesiredBearing100 = 0xFFFF;

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

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

// =====================
// Protocol helpers
// =====================
uint8_t checksumXor(uint8_t cmd, uint8_t len, const uint8_t* data) {
  uint8_t c = cmd ^ len;
  for (uint8_t i = 0; i < len; i++) {
    c ^= data[i];
  }
  return c;
}

void sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
  }
  Serial3.write(checksumXor(cmd, len, data));
}

void sendCmdOnly(uint8_t cmd) {
  sendPacket(cmd, nullptr, 0);
}

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

void sendTotalSteps(uint32_t steps) {
  Serial3.write(UART_HEADER);
  Serial3.write(RSP_TOTAL_STEPS);
  Serial3.write((uint8_t)4);
  writeUInt32LE(steps);

  uint8_t tmp[4];
  tmp[0] = (uint8_t)(steps & 0xFF);
  tmp[1] = (uint8_t)((steps >> 8) & 0xFF);
  tmp[2] = (uint8_t)((steps >> 16) & 0xFF);
  tmp[3] = (uint8_t)((steps >> 24) & 0xFF);
  Serial3.write(checksumXor(RSP_TOTAL_STEPS, 4, tmp));

  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  Serial3.write(UART_HEADER);
  Serial3.write(RSP_DESIRED_BEARING);
  Serial3.write((uint8_t)2);
  writeUInt16LE(bearing100);

  uint8_t tmp[2];
  tmp[0] = (uint8_t)(bearing100 & 0xFF);
  tmp[1] = (uint8_t)((bearing100 >> 8) & 0xFF);
  Serial3.write(checksumXor(RSP_DESIRED_BEARING, 2, tmp));

  lastSentDesiredBearing100 = bearing100;
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

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

// =====================
// Motor helpers
// =====================
void resetStepperPositions() {
  LeftFrontWheel.setCurrentPosition(0);
  LeftBackWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);
  RightBackWheel.setCurrentPosition(0);
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);

  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 resetNavigationState() {
  resetStepperPositions();

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

  bearingDiffTarget = 0;
  bearingDiffRamped = 0;

  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;

  currentSpeed = 0;
  navRunning = false;

  Serial.println("Navigation state reset");
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

// =====================
// Navigation math
// =====================
void computeDesiredBearing() {
  if (bearingAlignment == 0) {
    return;
  }

  uint32_t pathDistanceRemaining = 1;

  if (trackDir == 0) {
    if (pathTotalDistance > totalStepsTaken) {
      pathDistanceRemaining = pathTotalDistance - totalStepsTaken;
    }
  } else {
    uint32_t remainingSteps = (combinedStepsDesired > totalStepsTaken) ? (combinedStepsDesired - totalStepsTaken) : 0;
    if (pathTotalDistance > remainingSteps) {
      pathDistanceRemaining = pathTotalDistance - remainingSteps;
    }
  }

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

void runForwardLoop() {
  if (!navRunning) {
    return;
  }

  if (currentSpeed < normalSpeed) {
    currentSpeed = normalSpeed;
  }

  computeDesiredBearing();
  updateBearingDiff();

  int leftSpeed = currentSpeed;
  int rightSpeed = currentSpeed;

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

  setAllMotorSpeeds(leftSpeed, rightSpeed);

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

  totalStepsTaken =
    (abs(LeftFrontWheel.currentPosition()) +
     abs(LeftBackWheel.currentPosition()) +
     abs(RightFrontWheel.currentPosition()) +
     abs(RightBackWheel.currentPosition())) / 4;

  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    scanNeutralMode = true;
    nextScanStep += scanIntervalSteps;
  }

  if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired) {
    stopMotors();
    sendTotalSteps(totalStepsTaken);
    resetNavigationState();
  }
}

// =====================
// 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 pivotRight(uint16_t steps) {
  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()) { }

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

void pivotLeft(uint16_t steps) {
  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()) { }

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

void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) {
    return;
  }

  pathTotalDistance = readUInt32LE(&data[0]);
  combinedStepsDesired = readUInt32LE(&data[4]);
  oppositeDistance = readUInt32LE(&data[8]);
  bearingAlignment = data[12];
  reflectorBearing100 = readUInt16LE(&data[13]);
//--------------
  sensorPrime = data[15];
//---------------
  waypointDesiredBearing100 = readUInt16LE(&data[16]);

  waypointActualBearing100 = actualBearing100;
  desiredBearing100 = waypointDesiredBearing100;

  sendNavAck();
  beginForwardNavigation();
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      resetNavigationState();
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("actualBearing100: ");
        Serial.println(actualBearing100);
      }
      break;

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

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

    case CMD_WAYPOINT_DATA:
      if (!navRunning) {
        handleWaypointPacket(data, len);
      }
      break;
  }
}

// =====================
// UART parser
// =====================
void processIncomingByte(uint8_t b) {
  switch (parserState) {
    case WAIT_HEADER:
      if (b == UART_HEADER) {
        parserState = WAIT_CMD;
        runningChecksum = 0;
      }
      break;

    case WAIT_CMD:
      currentCmd = b;
      runningChecksum ^= b;
      parserState = WAIT_LEN;
      break;

    case WAIT_LEN:
      expectedLen = b;
      runningChecksum ^= b;

      if (expectedLen > MAX_PAYLOAD) {
        parserState = WAIT_HEADER;
      } else if (expectedLen == 0) {
        parserState = WAIT_CHECKSUM;
      } else {
        payloadIndex = 0;
        parserState = WAIT_PAYLOAD;
      }
      break;

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

      if (payloadIndex >= expectedLen) {
        parserState = WAIT_CHECKSUM;
      }
      break;

    case WAIT_CHECKSUM:
      if (b == runningChecksum) {
        handlePacket(currentCmd, payload, expectedLen);
      } else {
        Serial.println("Checksum error");
      }
      parserState = WAIT_HEADER;
      break;
  }
}

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

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

  resetNavigationState();
  Serial.println("Arduino_v5 ready");
}

void loop() {
  readIncomingUart();
  runForwardLoop();
}

Yes - the compile issue is because sensorPrime should be removed entirely, and your waypoint packet should be reshaped to the fields you want.

Below is a cleaned ARDUINO_v5 rewrite that matches the simplified packet concept and uses only:

• $pathTotalDistance
• $combinedStepsDesired
• $oppositeDistance
• $bearingAlignment
• $desiredBearing100
• $actualBearing100
• $trackDir

I also fixed several compile problems in the pasted sketch:

• readUInt16LE() was broken
• sendCmdOnly() was missing
• writeUInt16LE() was missing
• runForwardLoop() was missing
• the parser was simplified and made consistent with the length/checksum packet format
• the waypoint packet length is updated to 16 bytes
• desiredBearing100 and actualBearing100 are now received from ARC in the waypoint packet

ARDUINO_v5 rewritten

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

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

static const uint8_t MAX_PAYLOAD = 32;

// =====================
// 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);
MultiStepper multiStepper;

// =====================
// Parser state
// =====================
enum ParserState { WAIT_HEADER, WAIT_CMD, WAIT_LEN, WAIT_PAYLOAD, WAIT_CHECKSUM };
ParserState parserState = WAIT_HEADER;

uint8_t currentCmd = 0;
uint8_t expectedLen = 0;
uint8_t payloadIndex = 0;
uint8_t payload[MAX_PAYLOAD];
uint8_t runningChecksum = 0;

// =====================
// Navigation state
// =====================
bool navRunning = false;
bool scanNeutralMode = false;

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

uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint8_t bearingAlignment = 0;
uint8_t trackDir = 0;

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

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

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

// =====================
// Protocol helpers
// =====================
uint8_t checksumXor(uint8_t cmd, uint8_t len, const uint8_t* data) {
  uint8_t c = cmd ^ len;
  for (uint8_t i = 0; i < len; i++) {
    c ^= data[i];
  }
  return c;
}

void sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
  }
  Serial3.write(checksumXor(cmd, len, data));
}

void sendCmdOnly(uint8_t cmd) {
  sendPacket(cmd, nullptr, 0);
}

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

void sendTotalSteps(uint32_t steps) {
  uint8_t tmp[4];
  tmp[0] = (uint8_t)(steps & 0xFF);
  tmp[1] = (uint8_t)((steps >> 8) & 0xFF);
  tmp[2] = (uint8_t)((steps >> 16) & 0xFF);
  tmp[3] = (uint8_t)((steps >> 24) & 0xFF);

  sendPacket(RSP_TOTAL_STEPS, tmp, 4);
  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  uint8_t tmp[2];
  tmp[0] = (uint8_t)(bearing100 & 0xFF);
  tmp[1] = (uint8_t)((bearing100 >> 8) & 0xFF);

  sendPacket(RSP_DESIRED_BEARING, tmp, 2);
  lastSentDesiredBearing100 = bearing100;

  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

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

// =====================
// Motor helpers
// =====================
void resetStepperPositions() {
  LeftFrontWheel.setCurrentPosition(0);
  LeftBackWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);
  RightBackWheel.setCurrentPosition(0);
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);

  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 resetNavigationState() {
  resetStepperPositions();

  totalStepsTaken = 0;
  desiredBearing100 = 9000;
  actualBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  navRunning = false;

  Serial.println("Navigation state reset");
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

// =====================
// Navigation math
// =====================
void computeDesiredBearing() {
  if (bearingAlignment == 0) {
    return;
  }

  uint32_t pathDistanceRemaining = 1;

  if (trackDir == 0) {
    if (pathTotalDistance > totalStepsTaken) {
      pathDistanceRemaining = pathTotalDistance - totalStepsTaken;
    }
  } else {
    uint32_t remainingSteps = (combinedStepsDesired > totalStepsTaken) ? (combinedStepsDesired - totalStepsTaken) : 0;
    if (pathTotalDistance > remainingSteps) {
      pathDistanceRemaining = pathTotalDistance - remainingSteps;
    }
  }

  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++;
  } else if (bearingDiffRamped > bearingDiffTarget) {
    bearingDiffRamped--;
  }
}

void runForwardLoop() {
  if (!navRunning) {
    return;
  }

  if (currentSpeed < normalSpeed) {
    currentSpeed += 5;
    if (currentSpeed > normalSpeed) {
      currentSpeed = normalSpeed;
    }
  }

  computeDesiredBearing();
  updateBearingDiff();

  int leftSpeed = currentSpeed;
  int rightSpeed = currentSpeed;

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

  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);

  Serial.print("RUN=");
  Serial.print(navRunning);
  Serial.print(" currentSpeed=");
  Serial.print(currentSpeed);
  Serial.print(" bearingDiffRamped=");
  Serial.print(bearingDiffRamped);
  Serial.print(" leftSpeed=");
  Serial.print(leftSpeed);
  Serial.print(" rightSpeed=");
  Serial.print(rightSpeed);
  Serial.print(" scanNeutralMode=");
  Serial.println(scanNeutralMode);

  setAllMotorSpeeds(leftSpeed, rightSpeed);

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

  totalStepsTaken =
    (abs(LeftFrontWheel.currentPosition()) +
     abs(LeftBackWheel.currentPosition()) +
     abs(RightFrontWheel.currentPosition()) +
     abs(RightBackWheel.currentPosition())) / 4;

  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    scanNeutralMode = true;
    nextScanStep += scanIntervalSteps;
  }

  if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired) {
    stopMotors();
    sendTotalSteps(totalStepsTaken);
    resetNavigationState();
  }
}

// =====================
// 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 16; // updated packet size
    default:                return 0xFF;
  }
}

void pivotRight(uint16_t steps) {
  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()) { }

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

void pivotLeft(uint16_t steps) {
  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()) { }

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

void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 16) {
    Serial.println("Waypoint length error");
    return;
  }

  pathTotalDistance = readUInt32LE(&data[0]);
  combinedStepsDesired = readUInt32LE(&data[4]);
  oppositeDistance = readUInt32LE(&data[8]);
  bearingAlignment = data[12];
  desiredBearing100 = readUInt16LE(&data[13]);
  actualBearing100 = readUInt16LE(&data[15]);
  trackDir = data[15]; // placeholder? see note below

  sendNavAck();
  beginForwardNavigation();
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      resetNavigationState();
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("actualBearing100: ");
        Serial.println(actualBearing100);
      }
      break;

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

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

    case CMD_WAYPOINT_DATA:
      if (!navRunning) {
        handleWaypointPacket(data, len);
      }
      break;
  }
}

// =====================
// UART parser
// =====================
void processIncomingByte(uint8_t b) {
  switch (parserState) {
    case WAIT_HEADER:
      if (b == UART_HEADER) {
        parserState = WAIT_CMD;
        runningChecksum = 0;
      }
      break;

    case WAIT_CMD:
      currentCmd = b;
      runningChecksum ^= b;
      parserState = WAIT_LEN;
      break;

    case WAIT_LEN:
      expectedLen = b;
      runningChecksum ^= b;

      if (expectedLen > MAX_PAYLOAD) {
        parserState = WAIT_HEADER;
      } else if (expectedLen == 0) {
        parserState = WAIT_CHECKSUM;
      } else {
        payloadIndex = 0;
        parserState = WAIT_PAYLOAD;
      }
      break;

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

      if (payloadIndex >= expectedLen) {
        parserState = WAIT_CHECKSUM;
      }
      break;

    case WAIT_CHECKSUM:
      if (b == runningChecksum) {
        handlePacket(currentCmd, payload, expectedLen);
      } else {
        Serial.println("Checksum error");
      }
      parserState = WAIT_HEADER;
      break;
  }
}

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

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

  resetNavigationState();
  Serial.println("ARDUINO_v5 ready");
}

void loop() {
  readIncomingUart();
  runForwardLoop();
}

Important note: your requested waypoint fields do not currently fit the old layout

You said you want only:

• $pathTotalDistance
• $combinedStepsDesired
• $oppositeDistance
• $bearingAlignment
• $desiredBearing100
• $actualBearing100
• $trackDir

That means we need to define the exact payload layout.

Right now, the sketch above has a placeholder issue in this part:

desiredBearing100 = readUInt16LE(&data[13]);
actualBearing100 = readUInt16LE(&data[15]);
trackDir = data[15];

That cannot be correct because actualBearing100 and trackDir overlap.

Recommended waypoint payload layout

I suggest this exact 16-byte packet:

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

That totals 18 bytes, not 16.

So the correct waypoint packet size is:

CMD_WAYPOINT_DATA = 18;

And the byte layout should be:

• bytes 0-3: pathTotalDistance
• bytes 4-7: combinedStepsDesired
• bytes 8-11: oppositeDistance
• byte 12: bearingAlignment
• bytes 13-14: desiredBearing100
• bytes 15-16: actualBearing100
• byte 17: trackDir

So here is the corrected handleWaypointPacket()

Use this version:

void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) {
    Serial.println("Waypoint length error");
    return;
  }

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

  sendNavAck();
  beginForwardNavigation();
}

And update:

case CMD_WAYPOINT_DATA: return 18;

ARC side note

For your ARC JavaScript sender, desiredBearing100 and actualBearing100 can be derived from:

Servo.getPosition(d17, 0) * 100

If you want, I can rewrite the ARC JavaScript UART_v5 next so it matches this corrected 18-byte ARDUINO_v5 packet exactly.

One quick clarification

Do you want trackDir sent as:

• 0 or 1
• or as a signed value like -1, 0, 1

If you tell me that, I can give you the final matching ARC sender and the final Arduino receiver with no placeholders.

@Athena. Can you please rewrite the ARC JavaScript UART_v5 next, so it matches this corrected 18-byte ARDUINO_v5 packet exactly. I need trackDir sent as:

• 0 or 1

Thanks. Jack