Pivot Stepper Sketch Help

Absolutely - below is a cleaned-up rewrite that adds pivot behavior for CMD_RIGHT_PIVOT and CMD_LEFT_PIVOT while keeping the same 2-byte little-endian STEPS payload format.

I also kept the rule that pivot commands only run when navRunning == false.

What this version does

• CMD_RIGHT_PIVOT

  • Left motors run forward
  • Right motors run reverse
  • Stops after the received STEPS count is reached

• CMD_LEFT_PIVOT

  • Right motors run forward
  • Left motors run reverse
  • Stops after the received STEPS count is reached

• Uses the same packet layout:

  • HEADER
  • CMD
  • LEN = 2
  • PAYLOAD = steps little-endian
  • CHECKSUM

• Pivot motion is tracked in a separate state so it won’t interfere with forward navigation.

Rewritten sketch

#include <AccelStepper.h>
#include <MultiStepper.h>
#include <math.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;

// -------------------------
// Motor setup
// -------------------------
AccelStepper LeftFrontWheel(AccelStepper::DRIVER, 3, 6);
AccelStepper LeftBackWheel(AccelStepper::DRIVER, 2, 5);
AccelStepper RightFrontWheel(AccelStepper::DRIVER, 12, 13);
AccelStepper RightBackWheel(AccelStepper::DRIVER, 4, 7);
MultiStepper multiStepper;

// -------------------------
// Parser
// -------------------------
enum ParserState {
  WAIT_HEADER,
  WAIT_CMD,
  WAIT_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;
uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
int32_t oppositeDistance = 0;
uint32_t nextScanStep = 0;
uint32_t scanIntervalSteps = 3056;
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 = 600;

// -------------------------
// Pivot state
// -------------------------
enum PivotMode {
  PIVOT_NONE,
  PIVOT_RIGHT,
  PIVOT_LEFT
};

PivotMode pivotMode = PIVOT_NONE;
uint32_t pivotStepsDesired = 0;
uint32_t pivotStepsTaken = 0;
uint32_t lastPivotProgressSteps = 0;

static const uint16_t PIVOT_SPEED = 600;

// -------------------------
// 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 sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  uint8_t checksum = cmd ^ len;
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);

  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
    checksum ^= data[i];
  }

  Serial3.write(checksum);
}

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

void sendNavAck() {
  sendCmdOnly(RSP_NAV_ACK);
  Serial.println(F("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(F("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(F("TX: DESIRED BEARING x100 = "));
  Serial.println(bearing100);
}

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

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

uint32_t getAverageAbsStepperPosition() {
  uint32_t a = (uint32_t)labs(LeftFrontWheel.currentPosition());
  uint32_t b = (uint32_t)labs(LeftBackWheel.currentPosition());
  uint32_t c = (uint32_t)labs(RightFrontWheel.currentPosition());
  uint32_t d = (uint32_t)labs(RightBackWheel.currentPosition());
  return (a + b + c + d) / 4;
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, -2000, 2000);
  rightSpeed = constrain(rightSpeed, -2000, 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);

  pivotMode = PIVOT_NONE;
  pivotStepsDesired = 0;
  pivotStepsTaken = 0;
  lastPivotProgressSteps = 0;

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

void resetNavigationState() {
  resetStepperPositions();
  totalStepsTaken = 0;
  desiredBearing100 = 9000;
  actualBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  navRunning = false;
  Serial.println(F("Navigation state reset"));
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  nextScanStep = scanIntervalSteps;
  Serial.println(F("Navigation started"));
}

void beginPivot(PivotMode mode, uint32_t steps) {
  if (steps == 0) {
    Serial.println(F("Pivot ignored: steps = 0"));
    return;
  }

  pivotMode = mode;
  pivotStepsDesired = steps;
  pivotStepsTaken = 0;
  lastPivotProgressSteps = 0;

  if (mode == PIVOT_RIGHT) {
    // left wheels forward, right wheels reverse
    setAllMotorSpeeds((int)PIVOT_SPEED, -(int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(RIGHT)"));
  } else if (mode == PIVOT_LEFT) {
    // right wheels forward, left wheels reverse
    setAllMotorSpeeds(-(int)PIVOT_SPEED, (int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(LEFT)"));
  }
}

uint32_t getPivotProgressSteps() {
  uint32_t a = (uint32_t)labs(LeftFrontWheel.currentPosition());
  uint32_t b = (uint32_t)labs(LeftBackWheel.currentPosition());
  uint32_t c = (uint32_t)labs(RightFrontWheel.currentPosition());
  uint32_t d = (uint32_t)labs(RightBackWheel.currentPosition());
  return (a + b + c + d) / 4;
}

void updatePivotLoop() {
  if (pivotMode == PIVOT_NONE) {
    return;
  }

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

  pivotStepsTaken = getPivotProgressSteps();

  if (pivotStepsTaken >= pivotStepsDesired) {
    stopMotors();
    Serial.print(F("Pivot complete, steps="));
    Serial.println(pivotStepsTaken);
  }
}

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

  if (pathDistanceRemaining == 0) {
    pathDistanceRemaining = 1;
  }

  bool reflectorIsLeft = (desiredBearing100 >= 9100);
  int32_t signedOppositeDistance = (int32_t)oppositeDistance;

  if (reflectorIsLeft) {
    signedOppositeDistance = -labs(signedOppositeDistance);
  } else {
    signedOppositeDistance = labs(signedOppositeDistance);
  }

  float angleDeg = atan2((float)signedOppositeDistance, (float)pathDistanceRemaining) * 180.0f / PI;
  int32_t bearing100 = (int32_t)((90.0f + angleDeg) * 100.0f);

  if (bearing100 < 0) bearing100 = 0;
  if (bearing100 > 18000) bearing100 = 18000;

  desiredBearing100 = (uint16_t)bearing100;

  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 applyForwardMotion() {
  int leftSpeed = normalSpeed;
  int rightSpeed = normalSpeed;

  int absDiff = abs(bearingDiffRamped);

  if (bearingDiffRamped > 0) {
    leftSpeed = normalSpeed - (absDiff * 2);
    rightSpeed = normalSpeed + (absDiff * 2);
  } else if (bearingDiffRamped < 0) {
    leftSpeed = normalSpeed + (absDiff * 2);
    rightSpeed = normalSpeed - (absDiff * 2);
  }

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

  setAllMotorSpeeds(leftSpeed, rightSpeed);
}

void updateObstacleScanSchedule() {
  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    nextScanStep += scanIntervalSteps;
  }
}

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

  computeDesiredBearing();
  updateBearingDiff();
  applyForwardMotion();

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

  totalStepsTaken = getAverageAbsStepperPosition();
  updateObstacleScanSchedule();

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

// -------------------------
// Packet handlers
// -------------------------
void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 22) {
    Serial.print(F("Waypoint length error: "));
    Serial.println(len);
    return;
  }

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

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

  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(F("RX: ACTUAL BEARING x100 = "));
        Serial.println(actualBearing100);
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2 && !navRunning) {
        uint16_t steps = readUInt16LE(data);
        Serial.print(F("RX: RIGHT PIVOT steps="));
        Serial.println(steps);
        beginPivot(PIVOT_RIGHT, steps);
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && !navRunning) {
        uint16_t steps = readUInt16LE(data);
        Serial.print(F("RX: LEFT PIVOT steps="));
        Serial.println(steps);
        beginPivot(PIVOT_LEFT, steps);
      }
      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(F("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 entry points
// -------------------------
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);

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

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

  resetNavigationState();
  Serial.println(F("ARDUINO_v8 ready"));
}

void loop() {
  readIncomingUart();

  if (navRunning) {
    runForwardLoop();
  } else {
    updatePivotLoop();
  }
}

Important note about step counting

I preserved your 2-byte little-endian STEPS payload exactly as requested.

That said, for pivoting I used:

pivotStepsTaken = getPivotProgressSteps();

which averages the absolute position of all 4 motors.

Why I did that

Because during a pivot:

• left motors move one direction
• right motors move the opposite direction

So using absolute values makes the pivot progress count up regardless of direction.

One thing to consider

Your current sketch mixes:

• AccelStepper::runSpeed()
• stop()
• setSpeed()

That works for simple speed-based control, but it does not provide a true closed-loop "move exactly N steps" behavior by itself unless you stop based on your own position tracking like we did here.

If you want, I can also give you a more robust version that:

• explicitly stores the starting step positions at pivot start
• computes pivot progress as the delta from that starting point
• optionally keeps left/right motor pairs perfectly synchronized

That version is usually better for repeatable pivots.

ARC / EZB side note

Since the EZ-B is acting as a UART bridge, your ARC JavaScript should continue sending the same 2-byte payload format. If you want, I can also help you write the ARC JavaScript packet sender for:

• CMD_RIGHT_PIVOT
• CMD_LEFT_PIVOT

using Serial.write() / UART.hardwareUartWrite() style patterns in ARC.

Quick question

Do you want the pivot to stop based on:

1. average wheel step count as above, or
2. a stricter per-wheel delta so each motor must reach the target more precisely?

If you want, I can also rewrite this into a cleaner state-machine version with separate navMode and pivotMode handling.

@Athena, I set the Steps to 600. I call for Right Pivot first, then after a 5 sec delay call for Left Pivot. UART is sending the commands, and the Arduino is printing the received commands. The wheels however travel twice as far on the Left Pivot than the Right Pivot.

• Can you confirm that the Right and Left Pivots are clearing out received UART data and wheel positions after each Pivot?
• The print: "ARDUINO_v8 ready" only occasionally prints after uploading sketch. The earlier version of this sketch printed this reliably. I'm not sure if this irregularity means anything or not.

I need reasonable Step accuracy in distance traveled. I actually intend for inaccuracies to be managed by IR sensor repositioning. Jack

Yes - based on your description, there are two separate issues here:

1. The pivot state is not being fully reset between pivots
2. Your current step-count method is likely asymmetrical, which can easily make the left pivot travel farther than the right pivot

Also, the fact that ARDUINO_v8 ready only sometimes prints usually points to startup timing / reset behavior on Serial3 or USB serial, not necessarily a logic failure in the sketch.

1) Are UART buffers and wheel positions being cleared after each pivot?

UART data

No - not explicitly.
Your parser will continue reading bytes as they arrive, but the sketch currently does not clear any buffered UART data at the start or end of a pivot. That usually is not the direct cause of unequal pivot distance if packets are valid, but it can cause stale bytes to remain in the serial buffer if ARC/EZB sends bursts or if a partial packet gets left behind.

For cleaner behavior, I recommend calling:

• Serial3.flush() only if you are sure you want to wait for outgoing data to finish
• and more importantly, draining any pending input bytes before starting a new pivot

Example helper:

void clearIncomingUart()
{
  while (Serial3.available() > 0)
  {
    Serial3.read();
  }
}

Call that:

• before starting a pivot
• after completing a pivot
• optionally before processing a new command if you want a clean boundary

Wheel positions

No - not in a pivot-specific way.
In the version I gave you, pivot progress was being measured by absolute current step positions, but the step positions themselves were not reset at pivot start/end.

That means if one pivot begins with existing motor offsets, you can get inconsistent results.

For better repeatability, you should store the starting position of each wheel when the pivot begins, then measure delta from start.

That is the right way to get consistent step accuracy.

2) Why Left Pivot may travel farther than Right Pivot

The most likely reason is that the current progress check was based on this kind of logic:

• average of labs(currentPosition()) across all motors

That can be inaccurate if:

• one side starts from a nonzero offset
• stop() decelerates differently depending on direction
• left/right stepper wiring or direction signs differ
• the motors don’t maintain identical speed/accel behavior in both directions

Better approach

For pivots, store the starting position of each motor:

• pivotStartLF
• pivotStartLB
• pivotStartRF
• pivotStartRB

Then compute progress as:

• abs(currentPosition - startPosition) for each wheel
• average those values
• stop when the average reaches the requested steps

That gives much better symmetry.

3) Why ARDUINO_v8 ready only prints occasionally

This usually means one of these:

• the Arduino is resetting, but the serial monitor or USB serial isn’t ready at the exact moment the print occurs
• Serial3 or USB serial timing is inconsistent after upload
• the board is rebooting more than you expect due to USB connection state or startup power timing
• the sketch may be hanging very early if something else is interfering

Important

If you are viewing that print over the USB serial monitor, note that on Arduino Mega uploads often reset the board and the serial port may take a moment to reconnect. So an "occasional" startup message can happen even when the sketch is fine.

However, if the print was previously reliable and now isn’t, I would add a short startup delay before Serial.println():

delay(1000);
Serial.println(F("ARDUINO_v8 ready"));

That often stabilizes startup logging.

4) What I recommend changing

A. Add explicit pivot start positions

Use pivot-specific start values instead of raw absolute position.

B. Clear incoming UART before each pivot

This avoids stale packet fragments.

C. Reset pivot state after pivot completes

Set pivot mode back to none, zero out pivot counters, and optionally clear wheel positions if you want the next pivot measured fresh.

D. Add a startup delay before printing ready message

This should help the irregular startup print.

5) Example of the pivot logic you should use

Here is the core idea:

uint32_t pivotStartLF = 0;
uint32_t pivotStartLB = 0;
uint32_t pivotStartRF = 0;
uint32_t pivotStartRB = 0;

void beginPivot(PivotMode mode, uint32_t steps)
{
  if (steps == 0) return;

  clearIncomingUart();

  pivotMode = mode;
  pivotStepsDesired = steps;

  pivotStartLF = LeftFrontWheel.currentPosition();
  pivotStartLB = LeftBackWheel.currentPosition();
  pivotStartRF = RightFrontWheel.currentPosition();
  pivotStartRB = RightBackWheel.currentPosition();

  if (mode == PIVOT_RIGHT)
  {
    setAllMotorSpeeds((int)PIVOT_SPEED, -(int)PIVOT_SPEED);
  }
  else if (mode == PIVOT_LEFT)
  {
    setAllMotorSpeeds(-(int)PIVOT_SPEED, (int)PIVOT_SPEED);
  }
}

Then progress:

uint32_t getPivotProgressSteps()
{
  uint32_t lf = (uint32_t)labs(LeftFrontWheel.currentPosition() - pivotStartLF);
  uint32_t lb = (uint32_t)labs(LeftBackWheel.currentPosition() - pivotStartLB);
  uint32_t rf = (uint32_t)labs(RightFrontWheel.currentPosition() - pivotStartRF);
  uint32_t rb = (uint32_t)labs(RightBackWheel.currentPosition() - pivotStartRB);

  return (lf + lb + rf + rb) / 4;
}

This is much more reliable than using absolute positions.

6) Best practice for stopping pivots

Instead of relying only on stop() and then leaving the positions untouched, do this when pivot completes:

void finishPivot()
{
  stopMotors();
  resetStepperPositions();
  clearIncomingUart();
  pivotMode = PIVOT_NONE;
  pivotStepsDesired = 0;
  pivotStepsTaken = 0;
}

If you want the next pivot to start from a clean zero reference, this is a good choice.

7) My recommendation for your use case

Since you said:

I need reasonable Step accuracy in distance traveled. I intend for inaccuracies to be managed by IR sensor repositioning.

That means your firmware should aim for:

• consistent
• repeatable
• direction-symmetric movement, not necessarily perfect absolute precision.

So I would recommend:

• measure pivot progress from per-pivot start positions
• reset pivot state after every pivot
• drain UART input before starting pivot
• add the 1-second startup delay for reliable logging

8) Follow-up question

Do you want me to rewrite the full sketch now with these fixes applied?

I can give you a cleaned-up version that includes:

• per-pivot start position tracking
• UART buffer clearing
• pivot reset after completion
• startup delay for reliable ARDUINO_v8 ready
• improved step symmetry for right and left pivots

If you want, I’ll post the complete revised sketch in one block.

Yes @Athena, please give a cleaned-up version that includes:

• per-pivot start position tracking
• UART buffer clearing
• pivot reset after completion
• startup delay for reliable ARDUINO_v8 ready
• improved step symmetry for right and left pivots.

Let's call it Arduino_v9. Thank you. Jack

Absolutely, Jack - here’s a cleaned-up Arduino_v9 sketch with the fixes you asked for:

• per-pivot start position tracking
• UART buffer clearing
• pivot reset after completion
• startup delay for reliable ARDUINO_v9 ready
• improved step symmetry for right/left pivots

I also kept your 2-byte little-endian STEPS payload exactly as requested.

Arduino_v9

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

// =========================
// Motor setup
// =========================
AccelStepper LeftFrontWheel(AccelStepper::DRIVER, 3, 6);
AccelStepper LeftBackWheel(AccelStepper::DRIVER, 2, 5);
AccelStepper RightFrontWheel(AccelStepper::DRIVER, 12, 13);
AccelStepper RightBackWheel(AccelStepper::DRIVER, 4, 7);

MultiStepper multiStepper;

// =========================
// Parser
// =========================
enum ParserState {
  WAIT_HEADER,
  WAIT_CMD,
  WAIT_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;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
int32_t oppositeDistance = 0;
uint32_t nextScanStep = 0;
uint32_t scanIntervalSteps = 3056;
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 = 600;

// =========================
// Pivot state
// =========================
enum PivotMode {
  PIVOT_NONE,
  PIVOT_RIGHT,
  PIVOT_LEFT
};

PivotMode pivotMode = PIVOT_NONE;

uint32_t pivotStepsDesired = 0;
uint32_t pivotStepsTaken = 0;

long pivotStartLF = 0;
long pivotStartLB = 0;
long pivotStartRF = 0;
long pivotStartRB = 0;

static const uint16_t PIVOT_SPEED = 600;

// =========================
// 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 sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  uint8_t checksum = cmd ^ len;
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);

  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
    checksum ^= data[i];
  }

  Serial3.write(checksum);
}

void sendCmdOnly(uint8_t cmd) {
  uint8_t checksum = cmd ^ 0;
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write((uint8_t)0);
  Serial3.write(checksum);
}

void sendNavAck() {
  sendCmdOnly(RSP_NAV_ACK);
  Serial.println(F("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(F("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(F("TX: DESIRED BEARING x100 = "));
  Serial.println(bearing100);
}

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

void clearIncomingUart() {
  while (Serial3.available() > 0) {
    Serial3.read();
  }
}

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

void resetPivotState() {
  pivotMode = PIVOT_NONE;
  pivotStepsDesired = 0;
  pivotStepsTaken = 0;
  pivotStartLF = 0;
  pivotStartLB = 0;
  pivotStartRF = 0;
  pivotStartRB = 0;
}

uint32_t getAverageAbsStepperPosition() {
  uint32_t a = (uint32_t)labs(LeftFrontWheel.currentPosition());
  uint32_t b = (uint32_t)labs(LeftBackWheel.currentPosition());
  uint32_t c = (uint32_t)labs(RightFrontWheel.currentPosition());
  uint32_t d = (uint32_t)labs(RightBackWheel.currentPosition());
  return (a + b + c + d) / 4;
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, -2000, 2000);
  rightSpeed = constrain(rightSpeed, -2000, 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(F("Action: stopMotors()"));
}

void finishPivot() {
  stopMotors();
  clearIncomingUart();
  resetStepperPositions();
  resetPivotState();
  Serial.println(F("Pivot complete and reset"));
}

void resetNavigationState() {
  resetStepperPositions();
  totalStepsTaken = 0;
  desiredBearing100 = 9000;
  actualBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  navRunning = false;
  Serial.println(F("Navigation state reset"));
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  nextScanStep = scanIntervalSteps;
  Serial.println(F("Navigation started"));
}

void beginPivot(PivotMode mode, uint32_t steps) {
  if (steps == 0) {
    Serial.println(F("Pivot ignored: steps = 0"));
    return;
  }

  clearIncomingUart();
  resetStepperPositions();

  pivotMode = mode;
  pivotStepsDesired = steps;
  pivotStepsTaken = 0;

  pivotStartLF = LeftFrontWheel.currentPosition();
  pivotStartLB = LeftBackWheel.currentPosition();
  pivotStartRF = RightFrontWheel.currentPosition();
  pivotStartRB = RightBackWheel.currentPosition();

  if (mode == PIVOT_RIGHT) {
    // left wheels forward, right wheels reverse
    setAllMotorSpeeds((int)PIVOT_SPEED, -(int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(RIGHT)"));
  } else if (mode == PIVOT_LEFT) {
    // right wheels forward, left wheels reverse
    setAllMotorSpeeds(-(int)PIVOT_SPEED, (int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(LEFT)"));
  }
}

uint32_t getPivotProgressSteps() {
  uint32_t lf = (uint32_t)labs(LeftFrontWheel.currentPosition() - pivotStartLF);
  uint32_t lb = (uint32_t)labs(LeftBackWheel.currentPosition() - pivotStartLB);
  uint32_t rf = (uint32_t)labs(RightFrontWheel.currentPosition() - pivotStartRF);
  uint32_t rb = (uint32_t)labs(RightBackWheel.currentPosition() - pivotStartRB);
  return (lf + lb + rf + rb) / 4;
}

void updatePivotLoop() {
  if (pivotMode == PIVOT_NONE) {
    return;
  }

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

  pivotStepsTaken = getPivotProgressSteps();

  if (pivotStepsTaken >= pivotStepsDesired) {
    finishPivot();
    Serial.print(F("Pivot complete, steps="));
    Serial.println(pivotStepsTaken);
  }
}

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

  if (pathDistanceRemaining == 0) {
    pathDistanceRemaining = 1;
  }

  bool reflectorIsLeft = (desiredBearing100 >= 9100);
  int32_t signedOppositeDistance = (int32_t)oppositeDistance;

  if (reflectorIsLeft) {
    signedOppositeDistance = -labs(signedOppositeDistance);
  } else {
    signedOppositeDistance = labs(signedOppositeDistance);
  }

  float angleDeg = atan2((float)signedOppositeDistance, (float)pathDistanceRemaining) * 180.0f / PI;
  int32_t bearing100 = (int32_t)((90.0f + angleDeg) * 100.0f);

  if (bearing100 < 0) bearing100 = 0;
  if (bearing100 > 18000) bearing100 = 18000;

  desiredBearing100 = (uint16_t)bearing100;

  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 applyForwardMotion() {
  int16_t absDiff = abs(bearingDiffRamped);
  int leftSpeed = normalSpeed;
  int rightSpeed = normalSpeed;

  if (bearingDiffRamped > 0) {
    leftSpeed = normalSpeed - (absDiff * 2);
    rightSpeed = normalSpeed + (absDiff * 2);
  } else if (bearingDiffRamped < 0) {
    leftSpeed = normalSpeed + (absDiff * 2);
    rightSpeed = normalSpeed - (absDiff * 2);
  }

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

  setAllMotorSpeeds(leftSpeed, rightSpeed);
}

void updateObstacleScanSchedule() {
  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    nextScanStep += scanIntervalSteps;
  }
}

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

  computeDesiredBearing();
  updateBearingDiff();
  applyForwardMotion();

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

  totalStepsTaken = getAverageAbsStepperPosition();
  updateObstacleScanSchedule();

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

// =========================
// Packet handlers
// =========================
void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 22) {
    Serial.print(F("Waypoint length error: "));
    Serial.println(len);
    return;
  }

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

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

  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(F("RX: ACTUAL BEARING x100 = "));
        Serial.println(actualBearing100);
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2 && !navRunning) {
        uint16_t steps = readUInt16LE(data);
        Serial.print(F("RX: RIGHT PIVOT steps="));
        Serial.println(steps);
        beginPivot(PIVOT_RIGHT, steps);
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && !navRunning) {
        uint16_t steps = readUInt16LE(data);
        Serial.print(F("RX: LEFT PIVOT steps="));
        Serial.println(steps);
        beginPivot(PIVOT_LEFT, steps);
      }
      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(F("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 entry points
// =========================
void setup() {
  delay(1000); // give USB/serial time to settle
  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);

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

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

  resetNavigationState();
  resetPivotState();
  clearIncomingUart();

  Serial.println(F("ARDUINO_v9 ready"));
}

void loop() {
  readIncomingUart();

  if (navRunning) {
    runForwardLoop();
  } else {
    updatePivotLoop();
  }
}

Notes on the changes

1) Pivot symmetry

The biggest change is this:

• pivot progress is measured from per-pivot start positions
• not from raw absolute step counts

That should help a lot with the "right pivot behaves differently from left pivot" issue.

2) Pivot reset behavior

After a pivot completes, it now does all of this:

• stopMotors()
• clearIncomingUart()
• resetStepperPositions()
• resetPivotState()

So each pivot starts clean.

3) Startup message reliability

I added:

delay(1000);

before serial startup logging. That usually helps make the ARDUINO_v9 ready message much more consistent after upload/reset.

One important practical note

Because the motors are being driven open-loop, perfect matching of left/right pivot distance is still going to depend on:

• wheel traction
• load
• battery voltage
• motor driver consistency
• mechanical friction

So your plan to correct with IR repositioning is a good one.

If you want the next improvement

I can also make one more refinement for you:

• stop each pivot using a single shared pivot counter based only on one side, or
• use per-motor target positions instead of speed-running and averaging

That can make step accuracy even more predictable.

If you want, I can also help you with the ARC JavaScript packet sender for these pivot commands so it matches this sketch exactly.

Thank you @Athena. The Arduino_v9 works very well. Pivot distances are repeatable and exactly what I was looking for. Can you please review UART_v8, the (ARC JavaScript packet sender) for these pivot commands, so it matches this sketch exactly? Let's call this UART_v9. Thank you. Jack.

UART_v8

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

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

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER, cmd, payload.length]);
  if (payload.length > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
  }
  UART.hardwareUartWrite(UART_INDEX, [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);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(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 desiredBearing100 = clamp(safeInt(getVar("$desiredBearing100", 9000), 9000), 0, 18000);
  var actualBearing100 = clamp(safeInt(getVar("$actualBearing100", 9000), 9000), 0, 18000);
  var scanIntervalSteps = safeInt(getVar("$scanIntervalSteps", 3056), 3056) >>> 0;

  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);
  payload = payload.concat(u32ToBytes(scanIntervalSteps));

  if (payload.length !== 22) {
    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);
  print(" scanIntervalSteps=" + scanIntervalSteps);
}

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 requestObstacleScan() {
  if (getVar("$obsScanActive", 0) == 1) {
    print("RX: OBSTACLE SCAN REQUEST ignored (scan already active)");
    return;
  }

  setVar("$obsScanRequested", 1);
  setVar("$obsScanActive", 1);
  print("RX: OBSTACLE SCAN REQUEST -> $obsScanRequested=1, $obsScanActive=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);
      setVar("$legComplete", 1);
      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) {
    requestObstacleScan();
    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;
      rxState = (rxLen == 0) ? 4 : 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_v8 started");

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

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

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

Absolutely, Jack - here’s a cleaned-up UART_v9 that matches Arduino_v9 exactly for the pivot commands, including the same 2-byte little-endian STEPS payload.

What I changed

• Kept your packet format the same:
  • 0xA5 header
  • cmd
  • len
  • payload
  • checksum = cmd ^ len ^ payload...
• Made the pivot sender explicitly use the same 2-byte LE step format as Arduino_v9
• Cleaned up a few syntax issues in the pasted UART_v8 so it’s ARC JavaScript friendly
• Added a small helper to send any pivot command consistently
• Left the RX parsing behavior intact for the Arduino replies

UART_v9

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
// -------------------------
var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

// -------------------------
// Helpers
// -------------------------
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 checksum = checksumXor(cmd, len, payload);

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER, cmd, len]);
  if (len > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
  }
  UART.hardwareUartWrite(UART_INDEX, [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);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(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 desiredBearing100 = clamp(safeInt(getVar("$desiredBearing100", 9000), 9000), 0, 18000);
  var actualBearing100 = clamp(safeInt(getVar("$actualBearing100", 9000), 9000), 0, 18000);
  var scanIntervalSteps = safeInt(getVar("$scanIntervalSteps", 3056), 3056) >>> 0;

  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);
  payload = payload.concat(u32ToBytes(scanIntervalSteps));

  if (payload.length !== 22) {
    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);
  print(" scanIntervalSteps=" + scanIntervalSteps);
}

function sendPivot(cmd, name) {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  var payload = u16ToBytes(steps);

  sendPacket(cmd, payload);
  print("TX: " + name + " steps=" + steps);
}

function sendRightPivot() {
  sendPivot(CMD_RIGHT_PIVOT, "RIGHT PIVOT");
}

function sendLeftPivot() {
  sendPivot(CMD_LEFT_PIVOT, "LEFT PIVOT");
}

function requestObstacleScan() {
  if (getVar("$obsScanActive", 0) == 1) {
    print("RX: OBSTACLE SCAN REQUEST ignored (scan already active)");
    return;
  }

  setVar("$obsScanRequested", 1);
  setVar("$obsScanActive", 1);
  print("RX: OBSTACLE SCAN REQUEST -> $obsScanRequested=1, $obsScanActive=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);
      setVar("$legComplete", 1);
      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) {
    requestObstacleScan();
    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;
      rxState = (rxLen == 0) ? 4 : 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_v9 started");

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

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

  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

Pivot format matches Arduino_v9

This is the important part:

• CMD_RIGHT_PIVOT uses:
  • payload = [steps low byte, steps high byte]
• CMD_LEFT_PIVOT uses:
  • same exact format

So it will match the Arduino sketch exactly.

One small improvement

I added bytesToU32LE() because your RX handler used it but it wasn’t defined in the pasted script.

Suggested ARC variables

This script expects these variables to exist in your ARC project:

• $pivotSteps
• $sendRightPivotNow
• $sendLeftPivotNow
• $sendWaypointNow
• $waypointSent
• $sendStopNow
• $stopSent
• $actualBearing100
• $desiredBearing100
• $pathTotalDistance
• $combinedStepsDesired
• $oppositeDistance
• $bearingAlignment
• $trackDir
• $scanIntervalSteps
• $navAckReceived
• $totalStepsTaken
• $legComplete
• $obsScanRequested
• $obsScanActive

One thing to consider

Right now, this sender assumes your ARC script sets $pivotSteps before triggering either pivot command. If you want, I can also make a slightly nicer UART_v10 where:

• sendRightPivot(steps) and sendLeftPivot(steps) take the step count directly
• no dependency on $pivotSteps
• cleaner for calling from your ARC JavaScript

If you want that, I can rewrite it next.

Thank you @Athena, the UART_v9 pairs nicely with the Arduino_v9. I am experiencing difficulty with Arduino_v9 in the speed ramp-up function and the overall 'torquiness' of the motors. The ramp up to normal speed does not seem to work. Printing confirms that PWM 600 for left and right motors begin at start. [ applyForwardMotion()] Additionally, I wanted to soften the speed transition when a new actualBearing100 is received and so reduced the bearingDiffRamped to 1, from a 10. [updateBearingDiff()] I adjusted ARC script "IR_TRACKER_v3" to minimize changes to actualBearing100 to assist in minimizing the bearingDiffTarget in Arduino. Can you please review the posted Arduino_v9 below and rewrite it to smoothly ramp up to normal speed and not be so aggressive with PWM changes? Let's call it Arduino_v10. Thank you. Jack Arduino_v9

#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 -> ARCstatic 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;
// =========================// Motor setup// =========================AccelStepper LeftFrontWheel(AccelStepper::DRIVER, 3, 6);AccelStepper LeftBackWheel(AccelStepper::DRIVER, 2, 5);AccelStepper RightFrontWheel(AccelStepper::DRIVER, 12, 13);AccelStepper RightBackWheel(AccelStepper::DRIVER, 4, 7);
MultiStepper multiStepper;
// =========================// Parser// =========================enum ParserState {  WAIT_HEADER,  WAIT_CMD,  WAIT_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;
uint32_t totalStepsTaken = 0;uint32_t combinedStepsDesired = 0;uint32_t pathTotalDistance = 0;int32_t oppositeDistance = 0;uint32_t nextScanStep = 0;uint32_t scanIntervalSteps = 3056;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 = 600;
// =========================// Pivot state// =========================enum PivotMode {  PIVOT_NONE,  PIVOT_RIGHT,  PIVOT_LEFT};
PivotMode pivotMode = PIVOT_NONE;
uint32_t pivotStepsDesired = 0;uint32_t pivotStepsTaken = 0;
long pivotStartLF = 0;long pivotStartLB = 0;long pivotStartRF = 0;long pivotStartRB = 0;
static const uint16_t PIVOT_SPEED = 600;
// =========================// 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 sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {  uint8_t checksum = cmd ^ len;  Serial3.write(UART_HEADER);  Serial3.write(cmd);  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {    Serial3.write(data[i]);    checksum ^= data[i];  }
  Serial3.write(checksum);}
void sendCmdOnly(uint8_t cmd) {  uint8_t checksum = cmd ^ 0;  Serial3.write(UART_HEADER);  Serial3.write(cmd);  Serial3.write((uint8_t)0);  Serial3.write(checksum);}
void sendNavAck() {  sendCmdOnly(RSP_NAV_ACK);  Serial.println(F("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(F("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(F("TX: DESIRED BEARING x100 = "));  Serial.println(bearing100);}
void sendObstacleScanRequest() {  sendCmdOnly(RSP_OBS_SCAN_REQUEST);  Serial.println(F("TX: OBSTACLE SCAN REQUEST"));}
void clearIncomingUart() {  while (Serial3.available() > 0) {    Serial3.read();  }}
void resetStepperPositions() {  LeftFrontWheel.setCurrentPosition(0);  LeftBackWheel.setCurrentPosition(0);  RightFrontWheel.setCurrentPosition(0);  RightBackWheel.setCurrentPosition(0);}
void resetPivotState() {  pivotMode = PIVOT_NONE;  pivotStepsDesired = 0;  pivotStepsTaken = 0;  pivotStartLF = 0;  pivotStartLB = 0;  pivotStartRF = 0;  pivotStartRB = 0;}
uint32_t getAverageAbsStepperPosition() {  uint32_t a = (uint32_t)labs(LeftFrontWheel.currentPosition());  uint32_t b = (uint32_t)labs(LeftBackWheel.currentPosition());  uint32_t c = (uint32_t)labs(RightFrontWheel.currentPosition());  uint32_t d = (uint32_t)labs(RightBackWheel.currentPosition());  return (a + b + c + d) / 4;}
void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {  leftSpeed = constrain(leftSpeed, -2000, 2000);  rightSpeed = constrain(rightSpeed, -2000, 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(F("Action: stopMotors()"));}
void finishPivot() {  stopMotors();  clearIncomingUart();  resetStepperPositions();  resetPivotState();  Serial.println(F("Pivot complete and reset"));}
void resetNavigationState() {  resetStepperPositions();  totalStepsTaken = 0;  desiredBearing100 = 9000;  actualBearing100 = 9000;  lastSentDesiredBearing100 = 0xFFFF;  bearingDiffTarget = 0;  bearingDiffRamped = 0;  nextScanStep = scanIntervalSteps;  currentSpeed = 0;  navRunning = false;  Serial.println(F("Navigation state reset"));}
void beginForwardNavigation() {  navRunning = true;  currentSpeed = 0;  bearingDiffRamped = 0;  nextScanStep = scanIntervalSteps;  Serial.println(F("Navigation started"));}
void beginPivot(PivotMode mode, uint32_t steps) {  if (steps == 0) {    Serial.println(F("Pivot ignored: steps = 0"));    return;  }
  clearIncomingUart();  resetStepperPositions();
  pivotMode = mode;  pivotStepsDesired = steps;  pivotStepsTaken = 0;
  pivotStartLF = LeftFrontWheel.currentPosition();  pivotStartLB = LeftBackWheel.currentPosition();  pivotStartRF = RightFrontWheel.currentPosition();  pivotStartRB = RightBackWheel.currentPosition();
  if (mode == PIVOT_RIGHT) {    // left wheels forward, right wheels reverse    setAllMotorSpeeds((int)PIVOT_SPEED, -(int)PIVOT_SPEED);    Serial.println(F("Action: beginPivot(RIGHT)"));  } else if (mode == PIVOT_LEFT) {    // right wheels forward, left wheels reverse    setAllMotorSpeeds(-(int)PIVOT_SPEED, (int)PIVOT_SPEED);    Serial.println(F("Action: beginPivot(LEFT)"));  }}
uint32_t getPivotProgressSteps() {  uint32_t lf = (uint32_t)labs(LeftFrontWheel.currentPosition() - pivotStartLF);  uint32_t lb = (uint32_t)labs(LeftBackWheel.currentPosition() - pivotStartLB);  uint32_t rf = (uint32_t)labs(RightFrontWheel.currentPosition() - pivotStartRF);  uint32_t rb = (uint32_t)labs(RightBackWheel.currentPosition() - pivotStartRB);  return (lf + lb + rf + rb) / 4;}
void updatePivotLoop() {  if (pivotMode == PIVOT_NONE) {    return;  }
  LeftFrontWheel.runSpeed();  LeftBackWheel.runSpeed();  RightFrontWheel.runSpeed();  RightBackWheel.runSpeed();
  pivotStepsTaken = getPivotProgressSteps();
  if (pivotStepsTaken >= pivotStepsDesired) {    finishPivot();    Serial.print(F("Pivot complete, steps="));    Serial.println(pivotStepsTaken);  }}
// =========================// 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;    }  }
  if (pathDistanceRemaining == 0) {    pathDistanceRemaining = 1;  }
  bool reflectorIsLeft = (desiredBearing100 >= 9100);  int32_t signedOppositeDistance = (int32_t)oppositeDistance;
  if (reflectorIsLeft) {    signedOppositeDistance = -labs(signedOppositeDistance);  } else {    signedOppositeDistance = labs(signedOppositeDistance);  }
  float angleDeg = atan2((float)signedOppositeDistance, (float)pathDistanceRemaining) * 180.0f / PI;  int32_t bearing100 = (int32_t)((90.0f + angleDeg) * 100.0f);
  if (bearing100 < 0) bearing100 = 0;  if (bearing100 > 18000) bearing100 = 18000;
  desiredBearing100 = (uint16_t)bearing100;
  if (abs((int)desiredBearing100 - (int)lastSentDesiredBearing100) >= 100) {    sendDesiredBearing(desiredBearing100);  }}
void updateBearingDiff() {  bearingDiffTarget = (int16_t)actualBearing100 - (int16_t)desiredBearing100;
  if (bearingDiffRamped < bearingDiffTarget) {    bearingDiffRamped += 1;//10    if (bearingDiffRamped > bearingDiffTarget) {      bearingDiffRamped = bearingDiffTarget;    }  } else if (bearingDiffRamped > bearingDiffTarget) {    bearingDiffRamped -= 1;//10    if (bearingDiffRamped < bearingDiffTarget) {      bearingDiffRamped = bearingDiffTarget;    }  }}
void applyForwardMotion() {  int16_t absDiff = abs(bearingDiffRamped);  int leftSpeed = normalSpeed;  int rightSpeed = normalSpeed;
  if (bearingDiffRamped > 0) {    leftSpeed = normalSpeed - (absDiff * 2);    rightSpeed = normalSpeed + (absDiff * 2);  } else if (bearingDiffRamped < 0) {    leftSpeed = normalSpeed + (absDiff * 2);    rightSpeed = normalSpeed - (absDiff * 2);  }
//Serial.print("leftSpeed:  ");//Serial.println(leftSpeed);//Serial.print("rightSpeed:  ");//Serial.println(rightSpeed);

  leftSpeed = constrain(leftSpeed, 0, 2000);  rightSpeed = constrain(rightSpeed, 0, 2000);
  setAllMotorSpeeds(leftSpeed, rightSpeed);}
void updateObstacleScanSchedule() {  if (totalStepsTaken >= nextScanStep) {    sendObstacleScanRequest();    nextScanStep += scanIntervalSteps;  }}
void runForwardLoop() {  if (!navRunning) {    return;  }
  computeDesiredBearing();  updateBearingDiff();  applyForwardMotion();
  LeftFrontWheel.runSpeed();  LeftBackWheel.runSpeed();  RightFrontWheel.runSpeed();  RightBackWheel.runSpeed();
  totalStepsTaken = getAverageAbsStepperPosition();  updateObstacleScanSchedule();
  if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired) {    stopMotors();    sendTotalSteps(totalStepsTaken);    resetNavigationState();  }}
// =========================// Packet handlers// =========================void handleWaypointPacket(const uint8_t* data, uint8_t len) {  if (len != 22) {    Serial.print(F("Waypoint length error: "));    Serial.println(len);    return;  }
  pathTotalDistance = readUInt32LE(&data[0]);  combinedStepsDesired = readUInt32LE(&data[4]);  oppositeDistance = (int32_t)readUInt32LE(&data[8]);  bearingAlignment = data[12];  desiredBearing100 = readUInt16LE(&data[13]);  actualBearing100 = readUInt16LE(&data[15]);  trackDir = data[17] & 0x01;  scanIntervalSteps = readUInt32LE(&data[18]);
  Serial.println(F("RX: WAYPOINT DATA"));  Serial.print(F(" pathTotalDistance="));  Serial.println(pathTotalDistance);  Serial.print(F(" combinedStepsDesired="));  Serial.println(combinedStepsDesired);  Serial.print(F(" oppositeDistance="));  Serial.println(oppositeDistance);  Serial.print(F(" bearingAlignment="));  Serial.println(bearingAlignment);  Serial.print(F(" desiredBearing100="));  Serial.println(desiredBearing100);  Serial.print(F(" actualBearing100="));  Serial.println(actualBearing100);  Serial.print(F(" trackDir="));  Serial.println(trackDir);  Serial.print(F(" scanIntervalSteps="));  Serial.println(scanIntervalSteps);
  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(F("RX: ACTUAL BEARING x100 = "));        Serial.println(actualBearing100);      }      break;
    case CMD_RIGHT_PIVOT:      if (len == 2 && !navRunning) {        uint16_t steps = readUInt16LE(data);        Serial.print(F("RX: RIGHT PIVOT steps="));        Serial.println(steps);        beginPivot(PIVOT_RIGHT, steps);      }      break;
    case CMD_LEFT_PIVOT:      if (len == 2 && !navRunning) {        uint16_t steps = readUInt16LE(data);        Serial.print(F("RX: LEFT PIVOT steps="));        Serial.println(steps);        beginPivot(PIVOT_LEFT, steps);      }      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(F("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 entry points// =========================void setup() {  delay(1000); // give USB/serial time to settle  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);
  LeftFrontWheel.setSpeed(0);  LeftBackWheel.setSpeed(0);  RightFrontWheel.setSpeed(0);  RightBackWheel.setSpeed(0);
  multiStepper.addStepper(LeftFrontWheel);  multiStepper.addStepper(LeftBackWheel);  multiStepper.addStepper(RightFrontWheel);  multiStepper.addStepper(RightBackWheel);
  resetNavigationState();  resetPivotState();  clearIncomingUart();
  Serial.println(F("ARDUINO_v9 ready"));}
void loop() {  readIncomingUart();
  if (navRunning) {    runForwardLoop();  } else {    updatePivotLoop();  }}