Left Motors Stall Until Scan Request

Absolutely - below is a clean ARC JavaScript UART_v5 that matches the corrected 18-byte ARDUINO_v5 packet exactly, with trackDir sent as 0 or 1.

Packet layout

This sender uses the same payload structure you confirmed:

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

Total payload = 18 bytes

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;

// RX parser state
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)
  ) >>> 0;
}

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

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

  var out = [];
  out.push(UART_HEADER);
  out.push(cmd & 0xFF);
  out.push(len);

  for (var i = 0; i < payload.length; i++) {
    out.push(payload[i] & 0xFF);
  }

  out.push(cs);
  UART.hardwareUartWrite(UART_INDEX, out);
}

function sendCmdOnly(cmd) {
  sendPacket(cmd, []);
}

function sendStopMotors() {
  sendCmdOnly(CMD_STOP_MOTORS);
  print("TX: STOP MOTORS");
}

function sendActualBearing() {
  // You asked for bearing to reflect Servo.getPosition(d17,0) * 100
  var bearing100 = safeInt(Servo.getPosition(d17, 0), 90) * 100;
  bearing100 = clamp(bearing100, 0, 18000);

  var payload = u16ToBytes(bearing100);
  sendPacket(CMD_ACTUAL_BEARING, payload);

  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  // Bearing source per your request
  var bearing100 = safeInt(Servo.getPosition(d17, 0), 90) * 100;
  bearing100 = clamp(bearing100, 0, 18000);

  // Build packet values from ARC globals
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;

  // trackDir must be 0 or 1
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;

  // Per your request: both values reflect Servo.getPosition(d17,0) * 100
  var desiredBearing100 = bearing100;
  var actualBearing100 = bearing100;

  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload = payload.concat(u16ToBytes(actualBearing100));
  payload.push(trackDir);

  if (payload.length != 18) {
    print("Waypoint payload length error: " + payload.length);
    return;
  }

  sendPacket(CMD_WAYPOINT_DATA, payload);

  print("TX: WAYPOINT DATA");
  print("  pathTotalDistance=" + pathTotalDistance);
  print("  combinedStepsDesired=" + combinedStepsDesired);
  print("  oppositeDistance=" + oppositeDistance);
  print("  bearingAlignment=" + bearingAlignment);
  print("  desiredBearing100=" + desiredBearing100);
  print("  actualBearing100=" + actualBearing100);
  print("  trackDir=" + trackDir);
}

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

function sendLeftPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_LEFT_PIVOT, u16ToBytes(steps));
  print("TX: LEFT PIVOT steps=" + 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]);
      print("RX: TOTAL STEPS = " + steps);
      setVar("$totalStepsTaken", steps);
    }
    return;
  }

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

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

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

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

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

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

    if (rxState == 2) {
      rxLen = b & 0xFF;
      rxChecksum ^= rxLen;
      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();
    setVar("$sendRightPivotNow", 0);
  }

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

  processIncomingBytes();
  sleep(20);
}

Important correction on trackDir

I forced trackDir to be either:

• 0
• 1

with this logic:

trackDir = (trackDir === 0) ? 0 : 1;

So any nonzero value becomes 1.

One thing to verify

Your current Arduino packet layout is now:

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

That is 18 bytes, and the script above matches that.

ARC note

You’re using Servo.getPosition(d17, 0) * 100 as the bearing source. That works if d17 is your intended servo/bearing source in ARC. If you later want the real physical position of a smart servo, we can switch it to:

Servo.getPositionRealtime(d17, 0)

If you want, I can do the next step too

I can also rewrite ARDUINO_v5 to match this ARC sender exactly, including the 18-byte waypoint parser and the same checksum framing, so both sides are fully aligned.

@Athena. Can you please rewrite ARDUINO_v5 to match this ARC sender exactly, including the 18-byte waypoint parser and the same checksum framing, so both sides are fully aligned. Thanks. Jack.

Absolutely - below is a clean ARDUINO_v5 that matches the ARC sender exactly:

• Header: 0xA5
• Format: HEADER | CMD | LEN | PAYLOAD | CHECKSUM
• Checksum: XOR of CMD ^ LEN ^ all payload bytes
• Waypoint payload: exactly 18 bytes
• trackDir: 0 or 1
• desiredBearing100 / actualBearing100: received from ARC in the waypoint packet

ARDUINO_v5

#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 += 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;
  }

  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 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) {
    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] & 0x01;

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

  sendNavAck();
  beginForwardNavigation();
}

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;
      }
      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;
      runningChecksum = 0;
      currentCmd = 0;
      expectedLen = 0;
      payloadIndex = 0;
      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

This line in handleWaypointPacket() now treats trackDir as 0 or 1:

trackDir = data[17] & 0x01;

That means any incoming value will be reduced to either:

• 0
• 1

Packet layout used now

CMD_WAYPOINT_DATA payload = 18 bytes

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

One thing to verify in ARC

Your ARC sender currently says both of these come from:

Servo.getPosition(d17, 0) * 100

So desiredBearing100 and actualBearing100 will be identical unless you later separate them.

If that’s intentional, this Arduino version is aligned with it.

If you want, I can next give you:

1. a clean full ARC sender with comments removed and final variable names, or
2. a debug version of ARDUINO_v5 that prints raw packet bytes in hex so you can verify the serial framing live.

@Athena. One thing to note about $actualBearing100 and $desiredBearing100 in ARC (LEG_v5 script).

For WAYPOINT DATA, the initial $actualBearing100 and $desiredBearing100 are the same, Servo.getPosition(d17,0) * 100.

After this, ARC computes $actualBearing100 (IR_TRACKER script). ARDUINO computes $desiredBearing100. They are not read as Servo. positions, but rather (setVar) as global variables. ARC UART_v6 picks these global variables up as ARC and ARDUINO compute them.

Can you rewrite a clean full UART_ v6 with comments removed? Thanks. Jack

Sorry, I can't continue helping in this thread. It's grown long enough that my context window will require too much extra processing, which makes follow-ups unreliable and I can start making mistakes.

A couple of options:

• Start a new thread that picks up where we left off - fresh context, faster responses.
• Or get unlimited interaction with me through ARC's built-in AI Assistant agent: Synthiam AI Assistant

Thank you very much for your help. Jack