Arduino_V11 Symmetric PWM Steering

Absolutely - here’s a cleaner applyForwardMotion() that uses:

• a deadband to ignore tiny bearing errors
• a tunable proportional gain
• a maximum steering cap
• your symmetric steering behavior

Suggested constants

Add these near your steering constants:

static const int16_t BEARING_DEADBAND = 3;      // x100 units, so 3 = 0.03 degrees
static const int16_t STEERING_GAIN = 12;        // higher = stronger steering response
static const uint16_t MAX_BEARING_SPEED_ADJUST = 180;

Rewritten applyForwardMotion()

void applyForwardMotion() {
  // If we're holding steering during obstacle scan, keep motor speeds centered
  if (scanHoldSteering) {
    int32_t baseSpeed = rampedSpeed;

    if (baseSpeed > 2000) baseSpeed = 2000;
    if (baseSpeed < MIN_FORWARD_SPEED) baseSpeed = MIN_FORWARD_SPEED;

    setAllMotorSpeeds((int)baseSpeed, (int)baseSpeed);
    return;
  }

  // Difference is in x100 degrees
  int32_t diff = (int32_t)actualBearing100 - (int32_t)desiredBearing100;

  // Ignore tiny errors
  if (labs(diff) <= BEARING_DEADBAND) {
    diff = 0;
  }

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

  // Proportional steering:
  // steerAdjust grows as error grows
  int32_t steerAdjust = labs(diff) / STEERING_GAIN;

  // Clamp correction
  if (steerAdjust > MAX_BEARING_SPEED_ADJUST) {
    steerAdjust = MAX_BEARING_SPEED_ADJUST;
  }

  int32_t leftSpeed = baseSpeed;
  int32_t rightSpeed = baseSpeed;

  // Symmetric steering
  // Change sign here if your steering direction is reversed
  if (diff > 0) {
    leftSpeed -= steerAdjust;
    rightSpeed += steerAdjust;
  } else if (diff < 0) {
    leftSpeed += steerAdjust;
    rightSpeed -= steerAdjust;
  }

  // Final clamp
  if (leftSpeed > 2000) leftSpeed = 2000;
  if (rightSpeed > 2000) rightSpeed = 2000;
  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);
}

How to tune it

STEERING_GAIN

This controls how quickly steering responds to error.

• smaller number = stronger steering
• larger number = weaker steering

Examples:

• 8 = aggressive
• 12 = moderate
• 20 = gentle

BEARING_DEADBAND

This prevents the robot from constantly correcting tiny bearing noise.

Since your bearing is in x100 units:

• 3 = 0.03
• 50 = 0.5
• 100 = 1.0

For real robots, you may want something more like 50 to 150 depending on sensor noise.

Important note

If your steering still turns the wrong way, just swap the signs here:

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

to the opposite polarity.

If you want, I can also give you a full Arduino_v11c with this new steering function already integrated into the complete sketch.

Thank you @Athena for the update. Can you please give me a full Arduino_v11c with this new steering function already integrated into the complete sketch? Thanks. Jack

Absolutely - below is a full Arduino_v11c with the new proportional steering integrated, plus:

• deadband
• tunable steering gain
• max steering cap
• symmetrical left/right PWM correction
• scan hold steering so the robot does not keep correcting hard during an Obs Scan Request

I also kept the same packet/protocol structure you were using.

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

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;

// Steering tuning
static const int16_t BEARING_DEADBAND = 3;          // x100 units, 3 = 0.03 degrees
static const int16_t STEERING_GAIN = 12;            // higher = stronger steering response
static const uint16_t MAX_BEARING_SPEED_ADJUST = 180;

// Optional smoothing on bearing diff
static const int16_t BEARING_UPDATE_SMOOTH = 1;

// Steering polarity
// If the robot steers the wrong direction, change this from 1 to -1.
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()
{
  // Neutralize steering before scan request
  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;
  }
}

void applyForwardMotion()
{
  if (scanHoldSteering)
  {
    int32_t baseSpeed = rampedSpeed;
    if (baseSpeed > 2000) baseSpeed = 2000;
    if (baseSpeed < MIN_FORWARD_SPEED) baseSpeed = MIN_FORWARD_SPEED;

    setAllMotorSpeeds((int)baseSpeed, (int)baseSpeed);
    return;
  }

  int32_t baseSpeed = rampedSpeed;
  if (baseSpeed > 2000) baseSpeed = 2000;
  if (baseSpeed < MIN_FORWARD_SPEED) baseSpeed = MIN_FORWARD_SPEED;

  int16_t diff = bearingDiffRamped;

  int32_t steerAdjust = (labs(diff) * STEERING_GAIN) / 100;
  if (steerAdjust > MAX_BEARING_SPEED_ADJUST)
  {
    steerAdjust = MAX_BEARING_SPEED_ADJUST;
  }

  int32_t leftSpeed = baseSpeed;
  int32_t rightSpeed = baseSpeed;

  // Symmetric steering
  // If polarity is wrong, flip STEERING_POLARITY between 1 and -1.
  if (diff > 0)
  {
    leftSpeed -= (steerAdjust * STEERING_POLARITY);
    rightSpeed += (steerAdjust * STEERING_POLARITY);
  }
  else if (diff < 0)
  {
    leftSpeed += (steerAdjust * STEERING_POLARITY);
    rightSpeed -= (steerAdjust * STEERING_POLARITY);
  }

  leftSpeed = constrain(leftSpeed, -2000, 2000);
  rightSpeed = constrain(rightSpeed, -2000, 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);
        scanHoldSteering = false;   // resume steering once fresh bearing is received
        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_v11c ready"));
}

void loop()
{
  readIncomingUart();

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

What changed in v11c

1) Proportional steering

This line makes steering scale more cleanly:

int32_t steerAdjust = (labs(diff) * STEERING_GAIN) / 100;

So:

• bigger bearing error = stronger correction
• smaller bearing error = gentler correction
• STEERING_GAIN controls the strength

2) Deadband

Tiny errors are ignored:

if (bearingDiffTarget > -BEARING_DEADBAND && bearingDiffTarget < BEARING_DEADBAND)
{
  bearingDiffTarget = 0;
}

3) Scan hold steering

When the Arduino sends the obstacle scan request:

scanHoldSteering = true;
bearingDiffTarget = 0;
bearingDiffRamped = 0;

And when ARC sends a fresh bearing:

scanHoldSteering = false;

That prevents the robot from carrying over a stale steering correction during the scan.

Tuning tips

If steering feels too weak:

• increase STEERING_GAIN to 15, 20, or 25

If steering is too aggressive:

• reduce STEERING_GAIN
• reduce MAX_BEARING_SPEED_ADJUST

If it chatters near center:

• increase BEARING_DEADBAND

If the robot turns the wrong way:

• flip this:

static const int8_t STEERING_POLARITY = -1;

to:

static const int8_t STEERING_POLARITY = 1;

One important note

Your original snippet had a few corrupted fragments from copy/paste, so if you want, I can do one more pass and give you a fully compile-checked cleaned version with:

• consistent function names
• no duplicated logic
• easier tuning constants
• comments trimmed for readability

If you'd like, I can also write the matching ARC JavaScript that sends actualBearing100, waypoint, and Obs Scan Request packets cleanly.

Thank you @Athena for the update. However, normalSpeed (600) is never reached. The motors are stuck at MIN_FORWARD_SPEED (60).

Can you please rewrite the Arduino_v11c to correct the motor speed stuck at the lower limit? Can you please rewrite it as a fully compile-checked cleaned version? Please include:

• consistent function names
• no duplicated logic
• easier tuning constants
• comments trimmed for readability

Thanks. Jack

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

A couple of options:

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

Thank you @Athena for great help. Jack