Arduino_V11 Symmetric PWM Steering

Yes - the fix is to change the steering logic from single-side decay to balanced differential steering, where the same bearing correction is applied in opposite directions to the left and right motors.

What changes in Arduino_v11

Instead of doing this conceptually:

• Left motor gets reduced
• Right motor stays mostly the same

Arduino_v11 should do this:

• If bearing error says turn right:
  • Left speed increases
  • Right speed decreases
• If bearing error says turn left:
  • Left speed decreases
  • Right speed increases

That keeps the correction symmetric around the base forward speed.

Revised steering behavior

I’d replace your forward motion steering section with a centered differential mix:

leftSpeed  = baseSpeed + steeringAdjust;
rightSpeed = baseSpeed - steeringAdjust;

Then clamp both sides so neither exceeds limits.

Arduino_v11 example

Below is a cleaned-up rewrite of the steering-related parts of your sketch.
I’ve kept your protocol and navigation structure intact, and only changed the forward steering logic to be symmetric.

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

// =========================
// Forward ramp / smoothing
// =========================
uint16_t rampedSpeed = 0;
static const uint16_t MIN_FORWARD_SPEED = 60;
static const uint16_t SPEED_RAMP_STEP = 2;
static const uint16_t SPEED_DECAY_STEP = 2;
static const uint16_t MAX_BEARING_SPEED_ADJUST = 180;
static const int16_t BEARING_DEADBAND = 3;
static const int16_t BEARING_UPDATE_SMOOTH = 1;

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

int32_t abs32(int32_t v) {
  return v < 0 ? -v : v;
}

uint8_t clampU8(int32_t value, int32_t low, int32_t high) {
  if (value < low) return (uint8_t)low;
  if (value > high) return (uint8_t)high;
  return (uint8_t)value;
}

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 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;
  rampedSpeed = 0;
  navRunning = false;
  Serial.println(F("Navigation state reset"));
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  rampedSpeed = 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) {
    setAllMotorSpeeds((int)PIVOT_SPEED, -(int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(RIGHT)"));
  } else if (mode == PIVOT_LEFT) {
    setAllMotorSpeeds(-(int)PIVOT_SPEED, (int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(LEFT)"));
  }
}

uint32_t getPivotProgressSteps() {
  uint32_t lf = abs32(LeftFrontWheel.currentPosition() - pivotStartLF);
  uint32_t lb = abs32(LeftBackWheel.currentPosition() - pivotStartLB);
  uint32_t rf = abs32(RightFrontWheel.currentPosition() - pivotStartRF);
  uint32_t rb = abs32(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 (bearingDiffTarget > -BEARING_DEADBAND && bearingDiffTarget < BEARING_DEADBAND) {
    bearingDiffTarget = 0;
  }

  if (bearingDiffRamped < bearingDiffTarget) {
    bearingDiffRamped += BEARING_UPDATE_SMOOTH;
    if (bearingDiffRamped > bearingDiffTarget) bearingDiffRamped = bearingDiffTarget;
  } else if (bearingDiffRamped > bearingDiffTarget) {
    bearingDiffRamped -= BEARING_UPDATE_SMOOTH;
    if (bearingDiffRamped < bearingDiffTarget) bearingDiffRamped = bearingDiffTarget;
  }

  if (rampedSpeed < normalSpeed) {
    rampedSpeed += SPEED_RAMP_STEP;
    if (rampedSpeed > normalSpeed) rampedSpeed = normalSpeed;
  } else if (rampedSpeed > normalSpeed) {
    rampedSpeed -= SPEED_DECAY_STEP;
    if (rampedSpeed < normalSpeed) rampedSpeed = normalSpeed;
  }
}

// Symmetric steering: both sides move equally away from the base speed.
void applyForwardMotion() {
  int32_t diff = (int32_t)bearingDiffRamped;

  int32_t steerAdjust = abs(diff) * 2;
  if (steerAdjust > MAX_BEARING_SPEED_ADJUST) {
    steerAdjust = MAX_BEARING_SPEED_ADJUST;
  }

  int32_t baseSpeed = rampedSpeed;
  if (baseSpeed < MIN_FORWARD_SPEED) {
    baseSpeed = MIN_FORWARD_SPEED;
  }

  int32_t leftSpeed = baseSpeed;
  int32_t rightSpeed = baseSpeed;

  if (diff > 0) {
    // Need to turn right: slow right side, speed up left side equally
    leftSpeed += steerAdjust;
    rightSpeed -= steerAdjust;
  } else if (diff < 0) {
    // Need to turn left: slow left side, speed up right side equally
    leftSpeed -= steerAdjust;
    rightSpeed += steerAdjust;
  }

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

  setAllMotorSpeeds((int)leftSpeed, (int)rightSpeed);

  Serial.print(F("Steering diff="));
  Serial.print(diff);
  Serial.print(F(" base="));
  Serial.print(baseSpeed);
  Serial.print(F(" L="));
  Serial.print(leftSpeed);
  Serial.print(F(" R="));
  Serial.println(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"));

  sendCmdOnly(RSP_NAV_ACK);
  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);
        beginPivot(PIVOT_RIGHT, steps);
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && !navRunning) {
        uint16_t steps = readUInt16LE(data);
        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);

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

void loop() {
  readIncomingUart();

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

What specifically changed

The key difference is here:

if (diff > 0) {
  leftSpeed += steerAdjust;
  rightSpeed -= steerAdjust;
} else if (diff < 0) {
  leftSpeed -= steerAdjust;
  rightSpeed += steerAdjust;
}

That makes the correction symmetric.

In your earlier version, the steering likely behaved more like:

• leftSpeed = rampedSpeed - steerAdjust
• rightSpeed = rampedSpeed

which only decays one side.

Important note

Your posted sketch has a few formatting issues from the copy/paste, so I preserved the logic rather than trying to "compile-fix" every typo in the snippet. If you want, I can also give you a full compile-ready Arduino_v11 cleaned up line-by-line for the Mega/Due firmware.

ARC side

No ARC changes are required if your packet protocol stays the same.
You’d only need to keep sending the same bearing packets from ARC as before.

If you want, I can also help with:

1. a clean compile-ready Arduino_v11 sketch
2. the matching ARC JavaScript
3. a tunable steering version with parameters like:
   • steering gain
   • minimum turn speed
   • maximum correction clamp

If you’d like, I can produce the full Arduino_v11 file in one block next.

Thank you @Athena. Please write the full Arduino_v11 file in one block. Jack

Absolutely - below is a full Arduino_v11 sketch in one block, with the steering updated so left and right motor PWM are adjusted symmetrically based on bearing error.

I also cleaned up the sketch so it’s more internally consistent for Arduino Mega / Due EZB firmware use.

Note: Because your posted v10 snippet contained some copy/paste corruption in a few expressions, I reconstructed those parts into valid Arduino C++. If you already have a known-good v10 compile baseline, compare the protocol sections carefully.

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

// =========================
// Forward ramp / smoothing
// =========================
uint16_t rampedSpeed = 0;

static const uint16_t MIN_FORWARD_SPEED = 60;         // start moving gently
static const uint16_t SPEED_RAMP_STEP = 2;            // speed increase per update
static const uint16_t SPEED_DECAY_STEP = 2;           // speed decrease per update
static const uint16_t MAX_BEARING_SPEED_ADJUST = 180; // clamp steering effect
static const int16_t BEARING_DEADBAND = 3;            // ignore tiny bearing error
static const int16_t BEARING_UPDATE_SMOOTH = 1;       // ramp bearing error slowly

// =========================
// 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
// =========================
uint32_t abs32(long v) {
  return (v < 0) ? (uint32_t)(-v) : (uint32_t)v;
}

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 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;
  rampedSpeed = 0;
  navRunning = false;
  Serial.println(F("Navigation state reset"));
}

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  rampedSpeed = 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) {
    setAllMotorSpeeds((int)PIVOT_SPEED, -(int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(RIGHT)"));
  } else if (mode == PIVOT_LEFT) {
    setAllMotorSpeeds(-(int)PIVOT_SPEED, (int)PIVOT_SPEED);
    Serial.println(F("Action: beginPivot(LEFT)"));
  }
}

uint32_t getPivotProgressSteps() {
  uint32_t lf = abs32(LeftFrontWheel.currentPosition() - pivotStartLF);
  uint32_t lb = abs32(LeftBackWheel.currentPosition() - pivotStartLB);
  uint32_t rf = abs32(RightFrontWheel.currentPosition() - pivotStartRF);
  uint32_t rb = abs32(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 (bearingDiffTarget > -BEARING_DEADBAND && bearingDiffTarget < BEARING_DEADBAND) {
    bearingDiffTarget = 0;
  }

  if (bearingDiffRamped < bearingDiffTarget) {
    bearingDiffRamped += BEARING_UPDATE_SMOOTH;
    if (bearingDiffRamped > bearingDiffTarget) bearingDiffRamped = bearingDiffTarget;
  } else if (bearingDiffRamped > bearingDiffTarget) {
    bearingDiffRamped -= BEARING_UPDATE_SMOOTH;
    if (bearingDiffRamped < bearingDiffTarget) bearingDiffRamped = bearingDiffTarget;
  }

  if (rampedSpeed < normalSpeed) {
    rampedSpeed += SPEED_RAMP_STEP;
    if (rampedSpeed > normalSpeed) rampedSpeed = normalSpeed;
  } else if (rampedSpeed > normalSpeed) {
    rampedSpeed -= SPEED_DECAY_STEP;
    if (rampedSpeed < normalSpeed) rampedSpeed = normalSpeed;
  }
}

void applyForwardMotion() {
  int16_t diff = bearingDiffRamped;

  uint16_t steerAdjust = (uint16_t)abs(diff);
  if (steerAdjust > MAX_BEARING_SPEED_ADJUST) {
    steerAdjust = MAX_BEARING_SPEED_ADJUST;
  }

  int32_t baseSpeed = rampedSpeed;
  int32_t leftSpeed = baseSpeed;
  int32_t rightSpeed = baseSpeed;

  // Symmetric steering:
  // positive diff => robot is left of desired / needs to turn right
  // negative diff => robot is right of desired / needs to turn left
  if (diff > 0) {
    leftSpeed += steerAdjust;
    rightSpeed -= steerAdjust;
  } else if (diff < 0) {
    leftSpeed -= steerAdjust;
    rightSpeed += steerAdjust;
  }

  if (leftSpeed < 0) leftSpeed = 0;
  if (rightSpeed < 0) rightSpeed = 0;

  if (leftSpeed > 2000) leftSpeed = 2000;
  if (rightSpeed > 2000) rightSpeed = 2000;

  setAllMotorSpeeds((int)leftSpeed, (int)rightSpeed);

  Serial.print(F("NAV: base="));
  Serial.print(baseSpeed);
  Serial.print(F(" diff="));
  Serial.print(diff);
  Serial.print(F(" steer="));
  Serial.print(steerAdjust);
  Serial.print(F(" L="));
  Serial.print(leftSpeed);
  Serial.print(F(" R="));
  Serial.println(rightSpeed);
}

void updateObstacleScanSchedule() {
  if (scanIntervalSteps == 0) return;

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

  sendCmdOnly(RSP_NAV_ACK);
  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;

    default:
      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_v11 ready"));
}

void loop() {
  readIncomingUart();

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

Key steering change in v11

This is the important part:

if (diff > 0) {
  leftSpeed += steerAdjust;
  rightSpeed -= steerAdjust;
} else if (diff < 0) {
  leftSpeed -= steerAdjust;
  rightSpeed += steerAdjust;
}

That keeps the correction balanced, instead of decaying only one side.

One thing I recommend

Your steering will be easier to tune if you expose:

• MAX_BEARING_SPEED_ADJUST
• normalSpeed
• BEARING_DEADBAND
• BEARING_UPDATE_SMOOTH

If you want, I can also make Arduino_v11b with:

• a steering gain multiplier
• a minimum turning speed
• a soft saturation curve so it turns smoother near center and more aggressively at larger bearing errors

If helpful, I can also write the matching ARC JavaScript that sends the bearing packets for this v11 firmware.