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.