UART_V6 Subject Line Options:- UART_V6 Waypoint Be…

Yes - below is a cleaned-up ARC JavaScript UART_v6 script that matches your ARDUINO_v6 packet format and your clarification about WAYPOINT DATA:

• $desiredBearing100 is produced by the Arduino and sent back to ARC
• $actualBearing100 is produced in ARC as Servo.getPosition(d17, 0) * 100
• For WAYPOINT DATA only, ARC sends both values in the packet, and they are intentionally the same at transmit time

I also corrected a few syntax issues from the pasted draft so it will be valid ARC JavaScript.

UART_v6 ARC JavaScript

var UART_INDEX = 0;

var UART_HEADER = 0xA5;

var CMD_STOP_MOTORS = 0x01;
var CMD_ACTUAL_BEARING = 0x03;
var CMD_RIGHT_PIVOT = 0x04;
var CMD_LEFT_PIVOT = 0x05;
var CMD_WAYPOINT_DATA = 0x06;

var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;

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

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

function 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 packet = [];
  packet.push(UART_HEADER);
  packet.push(cmd & 0xFF);
  packet.push(payload.length & 0xFF);

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

  packet.push(checksumXor(cmd, payload.length, payload));

  UART.hardwareUartWrite(UART_INDEX, packet);
}

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

function sendActualBearing() {
  var bearing100 = safeInt(Servo.getPosition(d17, 0), 0) * 100;
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  setVar("$actualBearing100", bearing100);
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;

  var bearing100 = safeInt(Servo.getPosition(d17, 0), 0) * 100;
  var desiredBearing100 = bearing100;
  var actualBearing100 = bearing100;

  setVar("$actualBearing100", actualBearing100);
  setVar("$desiredBearing100", desiredBearing100);

  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] & 0xFF;
    available--;

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

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

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

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

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

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

    if (rxState == 4) {
      var cs = b & 0xFF;

      if (cs == (rxChecksum & 0xFF)) {
        handleRxPacket(rxCmd, rxPayload);
      } else {
        print("RX checksum error. cmd=" + rxCmd + " expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

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

print("UART_v6 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);
}

Notes

A couple of important ARC points:

• Servo.getPosition(d17, 0) is correct for ARC JavaScript
• I kept UART_INDEX = 0 since your Arduino sketch uses Serial3, but the ARC side depends on how you configured the hardware UART in ARC
• This script assumes the ARC robot skill is using the Hardware UART functions, not the older COM functions

One thing to verify

Your Arduino sketch uses:

• Serial3.begin(115200);

So in ARC, make sure the UART skill / configuration matches the same baud rate and the correct hardware UART mapping.

Quick sanity check on the packet format

This script matches your Arduino protocol:

• Header: 0xA5
• Command
• Length
• Payload
• Checksum XOR of cmd ^ len ^ payload bytes

And it uses these payload sizes:

• CMD_STOP_MOTORS = 0
• CMD_ACTUAL_BEARING = 2
• CMD_RIGHT_PIVOT = 2
• CMD_LEFT_PIVOT = 2
• CMD_WAYPOINT_DATA = 18

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

1. rewrite the Arduino sketch cleanly so it exactly matches this ARC script
2. verify the packet byte layout field-by-field
3. convert the ARC script into a reusable robot skill pattern
4. add debug prints for every incoming/outgoing packet

If you’d like, I can also show you where to place this script in ARC and how to configure the skill from the kebab menu.

@Athena. Here is what I meant to convey:

The function "sendActualBearing()" is sent anytime a new actualBearing is detected, except at initial start when the function "sendWaypointData()" is sent. Waypoint Data (actual bearing and desired bearing) will always be from the same servo position. "sendActualBearing()" will always be a computed bearing from ARC(not a servo position).

function sendActualBearing() {
 
 var bearing100 = getVar("$actualBearing100");  //****************************
  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);

Can you rewrite this code making certain to keep the 'safeInt' and 'clamp' safeguards in place if possible? Thank you. Jack

Absolutely - here’s the clean ARC JavaScript version with your requested behavior preserved:

• sendActualBearing() uses the computed ARC global $actualBearing100
• sendWaypointData() uses the same servo position source for both waypoint bearings
• safeInt() and clamp() are kept in place

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 sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);

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

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

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

  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;

  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;

  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(" desiredBearing100=" + desiredBearing100);
  print(" actualBearing100=" + actualBearing100);
}

Important note

Your line:

var bearing100 = safeInt(Servo.getPosition(d17, 0), 90) * 100;

is fine if you intentionally want a fallback of 90 degrees when the servo position is invalid. If you want the fallback to be a safer midpoint only when the value is missing, this is a good approach.

One small recommendation

Since Servo.getPosition(d17, 0) already returns a commanded position in degrees, using * 100 is correct for your x100 protocol.

If you want, I can also rewrite the full UART_v6 script again with this corrected behavior integrated everywhere so the whole file is consistent.

@Athena. Please rewrite the full UART_v6 script again with this corrected behavior integrated everywhere so the whole file is consistent. Thank you. Jack

Absolutely - below is a full, consistent ARC JavaScript UART_v6 script with your corrected behavior integrated:

• sendActualBearing() sends the computed ARC global $actualBearing100
• sendWaypointData() sends both waypoint bearings from the same servo position
• safeInt() and clamp() are preserved
• The protocol remains aligned with your Arduino sketch

UART_v6 - ARC JavaScript

var UART_INDEX = 0;
var UART_HEADER = 0xA5;

var CMD_STOP_MOTORS = 0x01;
var CMD_ACTUAL_BEARING = 0x03;
var CMD_RIGHT_PIVOT = 0x04;
var CMD_LEFT_PIVOT = 0x05;
var CMD_WAYPOINT_DATA = 0x06;

var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;

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

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

function clamp(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

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

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

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

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

function 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 == null) payload = [];

  var checksum = checksumXor(cmd, payload.length, payload);

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

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

  UART.hardwareUartWrite(UART_INDEX, [checksum]);

  print("TX: cmd=" + cmd + " len=" + payload.length + " cs=" + checksum);
}

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

function sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);

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

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

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

  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;

  var desiredBearing100 = bearing100;
  var actualBearing100 = bearing100;

  setVar("$desiredBearing100", desiredBearing100);
  setVar("$actualBearing100", actualBearing100);

  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 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] & 0xFF;
    available--;

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

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

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

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

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

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

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        handleRxPacket(rxCmd, rxPayload);
      } else {
        print("RX checksum error. cmd=" + rxCmd + " expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

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

print("UART_v6 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);
}

Two important notes

1. Servo.getPosition(d17, 0)

   • This is your waypoint source, and both desiredBearing100 and actualBearing100 are taken from it for the waypoint packet only.

2. $actualBearing100

   • This remains the computed ARC value used by sendActualBearing() later, not the servo position.

One thing I’d like to confirm

Your draft says:

var bearing100 = safeInt(Servo.getPosition(d17, 0), 90) * 100;

That’s valid if your servo position is in degrees and you want x100.

Do you want me to also make a matching cleaned Arduino_v6 sketch with the same naming and packet behavior so both sides are perfectly aligned?

@Athena. Please make a matching cleaned Arduino_v6 sketch with the same naming and packet behavior so both sides are perfectly aligned. Thank you. Jack

Absolutely - below is a cleaned Arduino_v6 sketch that matches the packet naming and behavior we’ve been using with your ARC UART_v6 script.

I kept the protocol the same:

• 0xA5 header
• command
• payload length
• payload bytes
• XOR checksum

And I aligned the behavior so:

• CMD_ACTUAL_BEARING is received as a computed bearing from ARC
• CMD_WAYPOINT_DATA contains both waypoint bearing values from the same servo position source
• Arduino replies with:
  • RSP_NAV_ACK
  • RSP_TOTAL_STEPS
  • RSP_DESIRED_BEARING
  • RSP_OBS_SCAN_REQUEST

Arduino_v6 Sketch

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

static const uint8_t MAX_PAYLOAD = 32;

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

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;

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

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

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

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

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;

  computeDesiredBearing();
  updateBearingDiff();

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

  multiStepper.moveTo(positions);
  while (multiStepper.run())
  {
  }

  totalStepsTaken++;

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

  if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired)
  {
    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 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("RX: ACTUAL BEARING x100 = ");
        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;
  }
}

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

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_v6 ready");
}

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

A couple of things to verify

• Serial3 wiring on the Mega is correct:
  • ARC TX Mega RX3
  • ARC RX Mega TX3
  • GND common between both
• Baud rate matches on both sides: 115200
• The ARC script and this Arduino sketch both use the same packet format and command IDs

One important note

Your current Arduino sketch calculates waypoint bearing from:

• pathTotalDistance
• combinedStepsDesired
• oppositeDistance
• bearingAlignment
• trackDir

That means the ARC side only needs to send valid values for those fields, plus the bearing pair.

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

1. clean the ARC UART_v6 script again one last time
2. merge both scripts into a final verified pair
3. add a packet-by-packet test sequence for ARC and Arduino
4. review the math for computeDesiredBearing() and updateBearingDiff()

If you'd like, I can also format both scripts into a copy/paste-ready final version with no explanatory text.

@Athena. Please review the math for computeDesiredBearing() and updateBearingDiff(). Thanks. Jack