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.

Thank you @Athena for Arduino_v11. The left motors and right motors now adjust speed in opposite directions equally, just as I have wanted for proportional steering. I did change the following, as the turns needed to be switched for correct steering.

// 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) { //-----------------------------------changed to <
    leftSpeed += steerAdjust;
    rightSpeed -= steerAdjust;
  } else if (diff < 0) {  //----------------------------changed to >
    leftSpeed -= steerAdjust;
    rightSpeed += steerAdjust;
  }

Was this change correct? Should there be any other code modifications for the steering direction to be changed?

One feature that seems to be missing from Arduino_v11 is the modification to bearing diff (and subsequent change to Left/Right motor speed) when the Arduino sends a UART request for an Obs Scan Request.

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

When an Obs Scan Request is made, the bearing diff remains at last setting and this motor correction proves too excessive during the Obs Scan, resulting in a deviation to the other side of the desired bearing. My solution would be to neutralize the bearing diff(zero) during the Obs Scan. Immediately after the Obs Scan is complete, ARC sends a new actual bearing to the Arduino for a new bearing diff calculation. Any suggestions for a more capable solution?

Could you please rewrite Arduino_v11 to reflect the change to Symmetric Steering and the change needed to neutralize bearing diff during a call for Obs Scan Request? Let's call it Arduino_v11b. Thanks. Jack

Yes - switching the steering signs can be correct, if your robot turns the wrong way with the original polarity.

1) Was your sign change correct?

If your original logic was:

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

and the robot turned opposite to what you expected, then reversing it to:

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

is the right fix provided that:

• a positive diff really means "the robot must steer the other way now"
• your left/right motor wiring or stepper direction mapping is inverted relative to the math

In practice

The most reliable test is:

• make diff > 0
• watch which way the robot turns
• if it turns the wrong way, flip the sign mapping

So yes, your change is a valid correction.

2) Is there anything else needed for steering direction?

Usually no, not if:

• the robot now turns correctly for positive and negative diff
• both sides still change symmetrically

But if behavior is still confusing, the better long-term approach is to create one tuning variable like:

int steeringPolarity = 1;

Then apply:

steerAdjust *= steeringPolarity;

That way you can invert steering without rewriting the logic.

3) About neutralizing bearing diff during obstacle scan

Yes, your idea is sound.

If the scan request is meant to pause or stabilize navigation behavior, then leaving the last steering correction active can cause the robot to drift during the scan. Neutralizing the bearing diff during the scan is a good approach.

Better ways to do it

You have a few options:

Option A - simplest

Set steering diff to zero when scan request is sent:

• bearingDiffTarget = 0;
• bearingDiffRamped = 0;
• optionally stop ramped steering influence for a short time

Option B - safer

Temporarily freeze steering correction during scan:

• introduce a flag like scanInProgress = true
• skip applyForwardMotion() steering correction while scanning
• restore correction when the new actual bearing arrives

Option C - best behavior

Treat scan as a brief steering hold state:

• preserve base forward motion if needed
• zero steering correction
• resume immediately once ARC sends the updated actual bearing

For your description, Option C is the most capable.

Arduino_v11b

Below is a rewrite with:

• symmetric steering
• your reversed steering polarity change
• bearing neutralization during Obs Scan Request
• a scanHoldSteering flag so steering stays neutral until a new actual bearing is received

// Arduino_v11b
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Arduino.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;
bool scanHoldSteering = false;   // NEW: freeze steering during obstacle scan

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;

// =========================
// Steering polarity
// =========================
// Set this to -1 if your robot turns the wrong way with the math.
static const int8_t STEERING_POLARITY = -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
// =========================
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) {
  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() {
  // NEW: neutralize steering before requesting scan
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  scanHoldSteering = true;

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

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  rampedSpeed = 0;
  bearingDiffRamped = 0;
  scanHoldSteering = false;
  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() {
  if (scanHoldSteering) {
    bearingDiffTarget = 0;
    bearingDiffRamped = 0;
    return;
  }

  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;
    if (rampedSpeed < MIN_FORWARD_SPEED) rampedSpeed = MIN_FORWARD_SPEED;
  } else if (rampedSpeed > normalSpeed) {
    rampedSpeed -= SPEED_DECAY_STEP;
    if (rampedSpeed < normalSpeed) rampedSpeed = normalSpeed;
  }
}

void applyForwardMotion() {
  int32_t diff = bearingDiffRamped;
  int32_t steerAdjust = abs(diff);

  if (steerAdjust > MAX_BEARING_SPEED_ADJUST) {
    steerAdjust = MAX_BEARING_SPEED_ADJUST;
  }

  // Apply your preferred reversed steering mapping here:
  // positive diff => steer one way, negative diff => steer the other way
  int32_t leftSpeed = rampedSpeed;
  int32_t rightSpeed = rampedSpeed;

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

  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(rampedSpeed);
  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);
        scanHoldSteering = false;   // NEW: resume steering after updated bearing arrives
        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);

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

void loop() {
  readIncomingUart();

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

Notes on this v11b version

Steering polarity

I added:

static const int8_t STEERING_POLARITY = -1;

That lets you flip the steering without rewriting your math again.

If the new version still turns the wrong direction, change it to:

static const int8_t STEERING_POLARITY = 1;

Scan hold behavior

This line is the new behavior:

scanHoldSteering = true;

When the scan request is sent, steering is neutralized.

Then when ARC sends a fresh actual bearing:

scanHoldSteering = false;

navigation steering resumes.

More capable solution suggestion

Your current idea is good, but the best long-term solution is:

• store the last valid bearing
• freeze steering during scan
• resume only after the new actual bearing arrives
• optionally ramp steering back in over a few cycles instead of restoring it instantly

That prevents a sudden correction jump after the scan.

If you want, I can make a v11c version that adds a smooth steering re-entry ramp after Obs Scan, so the motors don’t snap back to the new correction all at once.

If you'd like, I can also help you adapt the matching ARC JavaScript side so it coordinates the scan request and bearing update cleanly.

Thank you @Athena for the update. Can you please explain "MAX_BEARING_SPEED_ADJUST = 180"? Not much change in steering PWM takes place when this is changed. If the Max actualBearing100 sent by ARC producing a smaller than 18 bearing difference, will this automatically limit the effect of MAX_BEARING_SPEED_ADJUST? Thanks. Jack

Yes - MAX_BEARING_SPEED_ADJUST = 180 is only a cap, not the actual steering command.

What it means

In your steering math, the flow is basically:

1. ARC sends actualBearing100
2. Arduino computes diff = actualBearing100 - desiredBearing100
3. That diff is scaled into steerAdjust
4. steerAdjust is limited by MAX_BEARING_SPEED_ADJUST
5. Left/right PWM are offset by that amount

So MAX_BEARING_SPEED_ADJUST only matters after the bearing error has been converted into a speed adjustment.

Why changing it may not seem to do much

If your code is doing something like this:

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

then:

• a small diff gives a small steerAdjust
• a large diff gives a larger steerAdjust
• but it will never exceed 180

So if your bearing difference is only, say, 10 or 15, then changing the max from 180 to 250 won’t change anything because the correction is still only 10 or 15.

About your 18-degree question

If your actualBearing100 is measured in hundredths of a degree, then:

• 18 degrees = 1800
• 1.8 degrees = 180

So this line is important:

bearingDiffTarget = actualBearing100 - desiredBearing100;

If both values are in x100 units, then the diff is also in x100 units.

That means:

• diff = 180 means 1.80 degrees
• diff = 1800 means 18.00 degrees

So no, a smaller bearing difference does not automatically "limit" the effect of MAX_BEARING_SPEED_ADJUST in the sense of preventing steering. It simply means the correction is already smaller than the max cap.

The real issue: your code is likely not scaling diff into PWM strongly enough

Right now, if you’re only using the raw diff directly as the PWM correction, the effect may be too weak.

A better pattern is:

steerAdjust = abs(diff) / 10;

or

steerAdjust = map(abs(diff), 0, 3000, 0, MAX_BEARING_SPEED_ADJUST);

That way, the bearing error is converted into a more useful PWM correction range.

Example

If:

• desiredBearing100 = 9000
• actualBearing100 = 9180

then:

diff = 180

That is only 1.8 degrees.

If you use:

steerAdjust = abs(diff);

then steerAdjust is only 180, which may or may not be enough depending on your base speed.

But if your robot is already moving fast, 180 might be too small to visibly affect turning.

Recommended improvement

Use a proportional gain:

int32_t diff = (int32_t)actualBearing100 - (int32_t)desiredBearing100;
int32_t steerAdjust = abs(diff) / 10;   // tune this divisor

if (steerAdjust > MAX_BEARING_SPEED_ADJUST) {
  steerAdjust = MAX_BEARING_SPEED_ADJUST;
}

Then tune the divisor:

• / 5 = stronger steering
• / 10 = moderate steering
• / 20 = softer steering

Short answer

• MAX_BEARING_SPEED_ADJUST is only the maximum correction
• A smaller bearing difference will naturally produce a smaller correction
• It does not increase steering by itself
• If the steering response is weak, the problem is probably the scaling from diff to steerAdjust

If you want, I can rewrite your applyForwardMotion() function so it uses a cleaner proportional steering formula with a tunable gain and deadband.

Thank you @Athena for the detailed explanation. Can you please rewrite the applyForwardMotion() function, so it uses a cleaner proportional steering formula with a tunable gain and deadband. Thanks. Jack