Atan2 Bearing Geometry Fix

Yes - using atan2() is the better approach here, especially if you want your left/right geometry to be handled cleanly without manual quadrant fixes.

Recommended convention

I’d keep the angle math in this form:

• oppositeDistance > 0 = waypoint is to the right
• oppositeDistance < 0 = waypoint is to the left
• pathDistanceRemaining = forward distance component
• Bearing stays in your existing degrees × 100 format

That means the geometry becomes:

angleDeg = atan2(oppositeDistance, pathDistanceRemaining) * 180.0 / PI;
desiredBearing100 = (int16_t)((90.0 + angleDeg) * 100.0);

This gives you a natural center reference of 90.00, with left/right offsets applied automatically.

Full Arduino-form versions

computeDesiredBearing()

void computeDesiredBearing() {
  if (bearingAlignment == 0) return;

  // Determine remaining forward distance for the path
  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;
    }
  }

  // Avoid divide-by-zero and preserve signed left/right geometry
  if (pathDistanceRemaining == 0) {
    pathDistanceRemaining = 1;
  }

  // Use atan2 so oppositeDistance can be signed:
  //   positive = right
  //   negative = left
  float angleDeg = atan2((float)oppositeDistance, (float)pathDistanceRemaining) * 180.0f / PI;

  // Keep your existing 0..18000 scale, centered at 9000 = 90.00 degrees
  int32_t bearing100 = (int32_t)((90.0f + angleDeg) * 100.0f);

  // Clamp to valid range
  if (bearing100 < 0) bearing100 = 0;
  if (bearing100 > 18000) bearing100 = 18000;

  desiredBearing100 = (uint16_t)bearing100;

  // Only send if changed enough to matter
  if (abs((int)desiredBearing100 - (int)lastSentDesiredBearing100) >= 100) {
    sendDesiredBearing(desiredBearing100);
  }
}

updateBearingDiff()

void updateBearingDiff() {
  bearingDiffTarget = (int16_t)actualBearing100 - (int16_t)desiredBearing100;

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

Why this is better

atan2() advantages

• Handles all sign combinations correctly
• No special-case quadrant logic
• No divide-by-zero risk
• Easier to debug when waypoint geometry changes

Signed oppositeDistance

This is the key improvement. Instead of encoding direction in extra flags, the sign itself tells you direction.

One important note

If your oppositeDistance is currently always positive, then atan2() won’t know whether the waypoint is left or right. In that case, you’ll need to assign the sign before calling computeDesiredBearing().

For example:

if (waypointIsLeft) {
  oppositeDistance = -abs(oppositeDistance);
} else {
  oppositeDistance = abs(oppositeDistance);
}

Small bug I noticed in your sketch

Your helper function has a syntax issue here:

return (uint16_t)data[0] | ((uint16_t)data[1] > 8) & 0xFF));

It should be:

return (uint16_t)data[0] | ((uint16_t)data[1] << 8);

That matters because your current version would corrupt 16-bit packet values.

ARC-side note

Since you’re using a custom UART packet protocol on Serial3, this is a solid pattern for ARC integration. If you want, I can also help you convert this into:

• a clean ARC JavaScript sender
• or a custom ARC robot skill protocol handler

That would make the waypoint packets easier to test from ARC.

If you want, I can next rewrite your full waypoint packet format with the signed-distance fields and show the matching ARC JavaScript packet sender.

@Athena. Can you rewrite the full Arduino_v6 sketch to include the computeDesiredBearing() and the updateBearingDiff() sections that also incorporates the following:

if (waypointIsLeft) {
  oppositeDistance = -abs(oppositeDistance);
} else {
  oppositeDistance = abs(oppositeDistance);
}

Thank you. Jack

Absolutely - below is a cleaned-up Arduino_v6 version of the relevant logic with:

• atan2()-based bearing computation
• signed oppositeDistance using waypointIsLeft
• corrected readUInt16LE()
• updated computeDesiredBearing()
• updated updateBearingDiff()

I’ve kept the Serial3 UART protocol and your degrees x100 unit format.

Updated Arduino_v6 sketch sections

#include <AccelStepper.h>
#include <MultiStepper.h>

static const uint8_t UART_HEADER = 0xA5; // ARC -> Arduino

static const uint8_t CMD_STOP_MOTORS      = 0x01;
static const uint8_t CMD_ACTUAL_BEARING   = 0x03;
static const uint8_t CMD_RIGHT_PIVOT      = 0x04;
static const uint8_t CMD_LEFT_PIVOT       = 0x05;
static const uint8_t CMD_WAYPOINT_DATA    = 0x06;

// Arduino -> ARC
static const uint8_t RSP_NAV_ACK          = 0x10;
static const uint8_t RSP_TOTAL_STEPS      = 0x11;
static const uint8_t RSP_DESIRED_BEARING  = 0x12;
static const uint8_t RSP_OBS_SCAN_REQUEST = 0x13;

static const uint8_t MAX_PAYLOAD = 32;

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

enum ParserState {
  WAIT_HEADER,
  WAIT_CMD,
  WAIT_LEN,
  WAIT_PAYLOAD,
  WAIT_CHECKSUM
};

ParserState parserState = WAIT_HEADER;
uint8_t currentCmd = 0;
uint8_t expectedLen = 0;
uint8_t payloadIndex = 0;
uint8_t payload[MAX_PAYLOAD];
uint8_t runningChecksum = 0;

bool navRunning = false;
bool scanNeutralMode = false;
bool waypointIsLeft = false;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
int32_t oppositeDistance = 0;
uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint8_t bearingAlignment = 0;
uint8_t trackDir = 0;

uint16_t actualBearing100 = 9000;
uint16_t desiredBearing100 = 9000;
uint16_t lastSentDesiredBearing100 = 0xFFFF;

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

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

uint16_t readUInt16LE(const uint8_t* data) {
  return (uint16_t)data[0] |
         ((uint16_t)data[1] << 8);
}

void writeUInt32LE(uint32_t value) {
  Serial3.write((uint8_t)(value & 0xFF));
  Serial3.write((uint8_t)((value >> 8) & 0xFF));
  Serial3.write((uint8_t)((value >> 16) & 0xFF));
  Serial3.write((uint8_t)((value >> 24) & 0xFF));
}

uint8_t checksumXor(uint8_t cmd, uint8_t len, const uint8_t* data) {
  uint8_t c = cmd ^ len;
  for (uint8_t i = 0; i < len; i++) {
    c ^= data[i];
  }
  return c;
}

void sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
  }
  Serial3.write(checksumXor(cmd, len, data));
}

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

void sendNavAck() {
  sendCmdOnly(RSP_NAV_ACK);
  Serial.println("TX: NAV ACK");
}

void sendTotalSteps(uint32_t steps) {
  uint8_t tmp[4];
  tmp[0] = (uint8_t)(steps & 0xFF);
  tmp[1] = (uint8_t)((steps >> 8) & 0xFF);
  tmp[2] = (uint8_t)((steps >> 16) & 0xFF);
  tmp[3] = (uint8_t)((steps >> 24) & 0xFF);
  sendPacket(RSP_TOTAL_STEPS, tmp, 4);
  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  uint8_t tmp[2];
  tmp[0] = (uint8_t)(bearing100 & 0xFF);
  tmp[1] = (uint8_t)((bearing100 >> 8) & 0xFF);
  sendPacket(RSP_DESIRED_BEARING, tmp, 2);
  lastSentDesiredBearing100 = bearing100;
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

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

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

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);
  LeftFrontWheel.setSpeed(leftSpeed);
  LeftBackWheel.setSpeed(leftSpeed);
  RightFrontWheel.setSpeed(rightSpeed);
  RightBackWheel.setSpeed(rightSpeed);
}

void stopMotors() {
  LeftFrontWheel.stop();
  LeftBackWheel.stop();
  RightFrontWheel.stop();
  RightBackWheel.stop();

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

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

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

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

void computeDesiredBearing() {
  if (bearingAlignment == 0) return;

  uint32_t pathDistanceRemaining = 1;

  if (trackDir == 0) {
    if (pathTotalDistance > totalStepsTaken) {
      pathDistanceRemaining = pathTotalDistance - totalStepsTaken;
    }
  } else {
    uint32_t remainingSteps = (combinedStepsDesired > totalStepsTaken)
      ? (combinedStepsDesired - totalStepsTaken)
      : 0;

    if (pathTotalDistance > remainingSteps) {
      pathDistanceRemaining = pathTotalDistance - remainingSteps;
    }
  }

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

  if (waypointIsLeft) {
    oppositeDistance = -abs((long)oppositeDistance);
  } else {
    oppositeDistance = abs((long)oppositeDistance);
  }

  float angleDeg = atan2((float)oppositeDistance, (float)pathDistanceRemaining) * 180.0f / PI;

  int32_t bearing100 = (int32_t)((90.0f + angleDeg) * 100.0f);

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

  desiredBearing100 = (uint16_t)bearing100;

  if (abs((int)desiredBearing100 - (int)lastSentDesiredBearing100) >= 100) {
    sendDesiredBearing(desiredBearing100);
  }
}

void updateBearingDiff() {
  bearingDiffTarget = (int16_t)actualBearing100 - (int16_t)desiredBearing100;

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

void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) {
    Serial.println("Waypoint length error");
    return;
  }

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

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

  sendNavAck();
  beginForwardNavigation();
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      resetNavigationState();
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("RX: ACTUAL BEARING x100 = ");
        Serial.println(actualBearing100);
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2 && !navRunning) {
        // pivotRight(readUInt16LE(data));
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && !navRunning) {
        // pivotLeft(readUInt16LE(data));
      }
      break;

    case CMD_WAYPOINT_DATA:
      if (!navRunning) handleWaypointPacket(data, len);
      break;
  }
}

void processIncomingByte(uint8_t b) {
  switch (parserState) {
    case WAIT_HEADER:
      if (b == UART_HEADER) parserState = WAIT_CMD;
      break;

    case WAIT_CMD:
      currentCmd = b;
      runningChecksum = b;
      parserState = WAIT_LEN;
      break;

    case WAIT_LEN:
      expectedLen = b;
      runningChecksum ^= b;
      if (expectedLen > MAX_PAYLOAD) {
        parserState = WAIT_HEADER;
      } else if (expectedLen == 0) {
        parserState = WAIT_CHECKSUM;
      } else {
        payloadIndex = 0;
        parserState = WAIT_PAYLOAD;
      }
      break;

    case WAIT_PAYLOAD:
      payload[payloadIndex++] = b;
      runningChecksum ^= b;
      if (payloadIndex >= expectedLen) {
        parserState = WAIT_CHECKSUM;
      }
      break;

    case WAIT_CHECKSUM:
      if (b == runningChecksum) {
        handlePacket(currentCmd, payload, expectedLen);
      } else {
        Serial.println("Checksum error");
      }
      parserState = WAIT_HEADER;
      runningChecksum = 0;
      currentCmd = 0;
      expectedLen = 0;
      payloadIndex = 0;
      break;
  }
}

void readIncomingUart() {
  while (Serial3.available() > 0) {
    processIncomingByte((uint8_t)Serial3.read());
  }
}

void setup() {
  Serial.begin(115200);
  Serial3.begin(115200);

  LeftFrontWheel.setMaxSpeed(2000);
  LeftBackWheel.setMaxSpeed(2000);
  RightFrontWheel.setMaxSpeed(2000);
  RightBackWheel.setMaxSpeed(2000);

  LeftFrontWheel.setAcceleration(800);
  LeftBackWheel.setAcceleration(800);
  RightFrontWheel.setAcceleration(800);
  RightBackWheel.setAcceleration(800);

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

  resetNavigationState();
  Serial.println("ARDUINO_v6 ready");
}

void runForwardLoop() {
  computeDesiredBearing();
  updateBearingDiff();

  // Your forward motion logic goes here
}

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

Important note

I made oppositeDistance an int32_t because once you allow:

if (waypointIsLeft) {
  oppositeDistance = -abs(oppositeDistance);
} else {
  oppositeDistance = abs(oppositeDistance);
}

you want the variable to be signed everywhere it participates in the geometry.

One thing to verify

Right now I assumed:

• waypointIsLeft == true means the waypoint is left of current heading
• waypointIsLeft == false means right of current heading

If you want, I can also rewrite the ARC JavaScript sender so it transmits:

• oppositeDistance
• waypointIsLeft
• desiredBearing100
• actualBearing100

in a matching packet layout.

If you’d like, I can do the next pass and return this as a complete compile-ready .ino file with your pivotLeft(), pivotRight(), and forward drive logic included too.

@Athena.

One thing to verify

Right now, I assumed:

• waypointIsLeft == true means the waypoint is left of current heading
• waypointIsLeft == false means right of current heading

The variable waypointIsLeft is really the physical location of the reflector, and the term waypoint is more commonly associated with the destination of the rover. In the case of an 'offset' reflector, the reflector is not the destination. I would like to refer to waypointIsLeft as reflectorIsLeft.

However, in all situations desiredBearing can be used to determine if reflectorIsLeft is true.

• desiredBearing is 180...91degs = reflectorIsLeft is true.

• desiredBearing is 89...0degs = reflectorIsLeft is false.

• It would seem that reflectorIsLeft would not have to be UART sent at all and simply derived from desiredbearing in the Arduino.

If you agree with the above logic, can you rewrite 'void computeDesiredBearing()' reposted below to reflect the strategy of using desiredbearing as a flag for whether waypointIsLeft = true/false?

Thank you. Jack

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

  if (waypointIsLeft) {
    oppositeDistance = -abs((long)oppositeDistance);
  } else {
    oppositeDistance = abs((long)oppositeDistance);
  }

  float angleDeg = atan2((float)oppositeDistance, (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);
  }
}

Yes - that is a cleaner design.

If desiredBearing already tells you which side the reflector is on, then you do not need to UART-send a separate reflectorIsLeft flag. The Arduino can derive it locally from desiredBearing100.

Your logic is sound

Using your stated rule:

• desiredBearing100 from 18000..9100 reflectorIsLeft = true
• desiredBearing100 from 8900..0 reflectorIsLeft = false

That means the sign of oppositeDistance can be derived from desiredBearing100 itself.

Rewritten computeDesiredBearing()

Here is a clean version that uses desiredBearing100 as the flag source:

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

  // Derive reflector side from desired bearing:
  // 18000..9100 = left
  // 8900..0     = right
  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);
  }
}

Why this is better

This approach makes the Arduino behavior depend only on:

• the current desiredBearing100
• the geometry already known locally

So:

• fewer UART fields
• less packet complexity
• less chance of mismatch between ARC and Arduino
• easier debugging

Small caveat

Your threshold split at 9100 / 8900 leaves a small gap around 90.00.

If that gap was intentional for hysteresis, good. If not, you may want to make it a clean boundary like:

• desiredBearing100 >= 9000 left
• desiredBearing100 < 9000 right

That said, if you want some deadband around straight ahead, your current logic is reasonable.

One more suggestion

If desiredBearing100 is supposed to represent the measured angular direction to the reflector, I’d consider renaming the variable to something like:

• reflectorBearing100
• targetBearing100

That would make the code much easier to read later.

If you want, I can now rewrite the full Arduino_v6 sketch using this refined approach so reflectorIsLeft is fully derived locally and not transmitted at all.

@Athena. Thank you for confirming the logic of using desiredBearing100 for determining the geometry side for oppositeDistance.

This approach makes the Arduino behavior depend only on:

• the current desiredBearing100
• the geometry already known locally

So:

• fewer UART fields
• less packet complexity
• less chance of mismatch between ARC and Arduino
• easier debugging

Small caveat

Your threshold split at 9100 / 8900 leaves a small gap around 90.00.

Please leave this split. The desiredBearing of 90 will always be sent when 'WAYPOINT DATA' is sent upon initial powerup and bearingAlignment = 0. If bearingAlignment = 1, then desiredBearing will be 180...91deg OR 89...0deg.

One more suggestion

If desiredBearing100 is supposed to represent the measured angular direction to the reflector, I’d consider renaming the variable to something like:

• reflectorBearing100
• targetBearing100

Please leave the term desiredBearing100. I have been oversimplifying by defining it as the bearing to the reflector. It actually is not precisely, rather a 'modified bearing' to the reflector that ARC script "IR_TRACKER" uses in conjunction with the Arduino. The term "reflectorBearing" is actually used in another ARC script to create desiredBearing as a global variable.

The present Arduino sketch does not run the motors. This is a new development.

'void runForwardLoop()' is different by a lot from the previous 'void runForwardLoop()' . Can you rewrite the full Arduino_v7 sketch using this refined approach so that OppositeDistance geometry is derived from desiredBearing100 value and determine the problem with the motors not running?

Thank you. Jack

Absolutely - that makes sense, and keeping desiredBearing100 is the right call if it already has meaning in your ARC workflow.

The key change here is:

• Do not transmit a separate left/right flag
• Derive the geometry side from desiredBearing100 on the Arduino
• Keep your 9100 / 8900 split exactly as-is
• Investigate why the motors are not running by checking the stepper control flow

First: likely reason the motors are not running

From the sketch structure you posted earlier, the biggest issue is usually one of these:

1. runForwardLoop() is not actually commanding motion

If runForwardLoop() only computes bearing and updates variables, but never calls:

• moveTo(...)
• run()
• setSpeed(...)
• or some equivalent stepper drive loop

then the motors will not move.

2. MultiStepper only moves when you call moveTo() and run() properly

MultiStepper does not drive motors by itself. It requires:

• target positions
• repeated run() calls

If your forward-loop logic never updates a target position for the wheels, the steppers will sit still.

3. stopMotors() may be clearing speed too aggressively

If you call stopMotors() after every loop or after every state change, it can prevent sustained movement.

4. navRunning may be false

If your forward loop is guarded by if (!navRunning) return;, then confirm beginForwardNavigation() is actually reached.

5. The Arduino may be resetting navigation state too early

If you call resetNavigationState() after sendTotalSteps(), that can cancel motion state before the next motion segment is issued.

Updated computeDesiredBearing()

This version derives the side from desiredBearing100, exactly as you requested:

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

  // Keep your desiredBearing100 split exactly as requested:
  // 18000..9100 = reflector is left
  // 8900..0     = reflector is right
  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);
  }
}

Updated updateBearingDiff()

This remains the same conceptually:

void updateBearingDiff() {
  bearingDiffTarget = (int16_t)actualBearing100 - (int16_t)desiredBearing100;

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

Why the motors probably are not moving

Here is the likely mechanical/control issue:

MultiStepper is being used, but there is no motion target being set in runForwardLoop()

To move the motors, the code must do something like:

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

while (multiStepper.run()) {
  // keep stepping
}

If your runForwardLoop() does not produce a motion target or call run(), the motors will not advance.

Recommended runForwardLoop() structure

Since you said this version is very different from the old one, here is the control pattern I’d recommend:

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

  computeDesiredBearing();
  updateBearingDiff();

  // Example: you need some forward stepping target logic here
  // If no target is generated, motors will not move.
  //
  // This is where your speed correction / path tracking logic
  // should compute step targets and apply them.

  if (currentSpeed == 0) {
    currentSpeed = normalSpeed;
    setAllMotorSpeeds(currentSpeed, currentSpeed);
  }

  // If you want continuous stepper motion, you must call stepper movement here
  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();
}

Important note

If you are using runSpeed(), then you must set speeds first with setSpeed() and keep calling runSpeed() repeatedly.

If you are using MultiStepper, then you must update target positions and call run() until the movement completes.

My diagnosis

Based on what you described, I think the motor issue is probably:

• the geometry and bearing math are fine
• but the new runForwardLoop() does not generate actual wheel motion
• or it computes state but never feeds it into the stepper driver

So the fix is not in atan2() itself - it is in the motion execution part of the loop.

Next step

If you want, I can rewrite the full Arduino_v7 sketch in a clean compile-ready form, including:

• your UART packet parser
• desiredBearing100-based side detection
• corrected bearing computation
• a proper forward motor execution loop
• safer stepper state handling
• clearer separation between:
  • navigation math
  • motion output
  • UART reporting

If you paste your current runForwardLoop() and any missing motion functions, I can identify exactly why the motors are not running and fold the fix directly into the full sketch.

#include <AccelStepper.h>#include <MultiStepper.h>
static const uint8_t UART_HEADER = 0xA5; // ARC -> Arduino
static const uint8_t CMD_STOP_MOTORS      = 0x01;static const uint8_t CMD_ACTUAL_BEARING   = 0x03;static const uint8_t CMD_RIGHT_PIVOT      = 0x04;static const uint8_t CMD_LEFT_PIVOT       = 0x05;static const uint8_t CMD_WAYPOINT_DATA    = 0x06;
// Arduino -> ARCstatic const uint8_t RSP_NAV_ACK          = 0x10;static const uint8_t RSP_TOTAL_STEPS      = 0x11;static const uint8_t RSP_DESIRED_BEARING  = 0x12;static const uint8_t RSP_OBS_SCAN_REQUEST = 0x13;
static const uint8_t MAX_PAYLOAD = 32;
AccelStepper LeftFrontWheel(AccelStepper::DRIVER, 3, 6);AccelStepper LeftBackWheel(AccelStepper::DRIVER, 2, 5);AccelStepper RightFrontWheel(AccelStepper::DRIVER, 12, 13);AccelStepper RightBackWheel(AccelStepper::DRIVER, 4, 7);MultiStepper multiStepper;
enum ParserState {  WAIT_HEADER,  WAIT_CMD,  WAIT_LEN,  WAIT_PAYLOAD,  WAIT_CHECKSUM};
ParserState parserState = WAIT_HEADER;uint8_t currentCmd = 0;uint8_t expectedLen = 0;uint8_t payloadIndex = 0;uint8_t payload[MAX_PAYLOAD];uint8_t runningChecksum = 0;
bool navRunning = false;bool scanNeutralMode = false;bool waypointIsLeft = false;
uint32_t totalStepsTaken = 0;uint32_t combinedStepsDesired = 0;uint32_t pathTotalDistance = 0;int32_t oppositeDistance = 0;uint32_t nextScanStep = 10500;const uint32_t scanIntervalSteps = 10500;
uint8_t bearingAlignment = 0;uint8_t trackDir = 0;
uint16_t actualBearing100 = 9000;uint16_t desiredBearing100 = 9000;uint16_t lastSentDesiredBearing100 = 0xFFFF;
int16_t bearingDiffTarget = 0;int16_t bearingDiffRamped = 0;
uint16_t currentSpeed = 0;uint16_t normalSpeed = 800;
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);}
uint16_t readUInt16LE(const uint8_t* data) {  return (uint16_t)data[0] |         ((uint16_t)data[1] << 8);}

void writeUInt32LE(uint32_t value) {  Serial3.write((uint8_t)(value & 0xFF));  Serial3.write((uint8_t)((value >> 8) & 0xFF));  Serial3.write((uint8_t)((value >> 16) & 0xFF));  Serial3.write((uint8_t)((value >> 24) & 0xFF));}
uint8_t checksumXor(uint8_t cmd, uint8_t len, const uint8_t* data) {  uint8_t c = cmd ^ len;  for (uint8_t i = 0; i < len; i++) {    c ^= data[i];  }  return c;}
void sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {  Serial3.write(UART_HEADER);  Serial3.write(cmd);  Serial3.write(len);  for (uint8_t i = 0; i < len; i++) {    Serial3.write(data[i]);  }  Serial3.write(checksumXor(cmd, len, data));}
void sendCmdOnly(uint8_t cmd) {  sendPacket(cmd, nullptr, 0);}
void sendNavAck() {  sendCmdOnly(RSP_NAV_ACK);  Serial.println("TX: NAV ACK");}
void sendTotalSteps(uint32_t steps) {  uint8_t tmp[4];  tmp[0] = (uint8_t)(steps & 0xFF);  tmp[1] = (uint8_t)((steps >> 8) & 0xFF);  tmp[2] = (uint8_t)((steps >> 16) & 0xFF);  tmp[3] = (uint8_t)((steps >> 24) & 0xFF);  sendPacket(RSP_TOTAL_STEPS, tmp, 4);  Serial.print("TX: TOTAL STEPS = ");  Serial.println(steps);}
void sendDesiredBearing(uint16_t bearing100) {  uint8_t tmp[2];  tmp[0] = (uint8_t)(bearing100 & 0xFF);  tmp[1] = (uint8_t)((bearing100 >> 8) & 0xFF);  sendPacket(RSP_DESIRED_BEARING, tmp, 2);  lastSentDesiredBearing100 = bearing100;  Serial.print("TX: DESIRED BEARING x100 = ");  Serial.println(bearing100);}
void sendObstacleScanRequest() {  sendCmdOnly(RSP_OBS_SCAN_REQUEST);  Serial.println("TX: OBSTACLE SCAN REQUEST");}
void resetStepperPositions() {  LeftFrontWheel.setCurrentPosition(0);  LeftBackWheel.setCurrentPosition(0);  RightFrontWheel.setCurrentPosition(0);  RightBackWheel.setCurrentPosition(0);}
void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {  leftSpeed = constrain(leftSpeed, 0, 2000);  rightSpeed = constrain(rightSpeed, 0, 2000);  LeftFrontWheel.setSpeed(leftSpeed);  LeftBackWheel.setSpeed(leftSpeed);  RightFrontWheel.setSpeed(rightSpeed);  RightBackWheel.setSpeed(rightSpeed);}
void stopMotors() {  LeftFrontWheel.stop();  LeftBackWheel.stop();  RightFrontWheel.stop();  RightBackWheel.stop();
  LeftFrontWheel.setSpeed(0);  LeftBackWheel.setSpeed(0);  RightFrontWheel.setSpeed(0);  RightBackWheel.setSpeed(0);
  Serial.println("Action: stopMotors()");}
void resetNavigationState() {  resetStepperPositions();  totalStepsTaken = 0;  desiredBearing100 = 9000;  actualBearing100 = 9000;  lastSentDesiredBearing100 = 0xFFFF;  bearingDiffTarget = 0;  bearingDiffRamped = 0;  scanNeutralMode = false;  waypointIsLeft = false;  nextScanStep = scanIntervalSteps;  currentSpeed = 0;  navRunning = false;  Serial.println("Navigation state reset");}
void beginForwardNavigation() {  navRunning = true;  currentSpeed = 0;  bearingDiffRamped = 0;  scanNeutralMode = false;  nextScanStep = scanIntervalSteps;}
void computeDesiredBearing() {  if (bearingAlignment == 0) return;
  uint32_t pathDistanceRemaining = 1;
  if (trackDir == 0) {    if (pathTotalDistance > totalStepsTaken) {      pathDistanceRemaining = pathTotalDistance - totalStepsTaken;    }  } else {    uint32_t remainingSteps = (combinedStepsDesired > totalStepsTaken)      ? (combinedStepsDesired - totalStepsTaken)      : 0;
    if (pathTotalDistance > remainingSteps) {      pathDistanceRemaining = pathTotalDistance - remainingSteps;    }  }
  if (pathDistanceRemaining == 0) {    pathDistanceRemaining = 1;  }
  if (waypointIsLeft) {    oppositeDistance = -abs((long)oppositeDistance);  } else {    oppositeDistance = abs((long)oppositeDistance);  }
  float angleDeg = atan2((float)oppositeDistance, (float)pathDistanceRemaining) * 180.0f / PI;
  int32_t bearing100 = (int32_t)((90.0f + angleDeg) * 100.0f);
  if (bearing100 < 0) bearing100 = 0;  if (bearing100 > 18000) bearing100 = 18000;
  desiredBearing100 = (uint16_t)bearing100;
  if (abs((int)desiredBearing100 - (int)lastSentDesiredBearing100) >= 100) {    sendDesiredBearing(desiredBearing100);  }}
void updateBearingDiff() {  bearingDiffTarget = (int16_t)actualBearing100 - (int16_t)desiredBearing100;
  if (bearingDiffRamped < bearingDiffTarget) {    bearingDiffRamped += 10;    if (bearingDiffRamped > bearingDiffTarget) {      bearingDiffRamped = bearingDiffTarget;    }  } else if (bearingDiffRamped > bearingDiffTarget) {    bearingDiffRamped -= 10;    if (bearingDiffRamped < bearingDiffTarget) {      bearingDiffRamped = bearingDiffTarget;    }  }}
void handleWaypointPacket(const uint8_t* data, uint8_t len) {  if (len != 18) {    Serial.println("Waypoint length error");    return;  }
  pathTotalDistance = readUInt32LE(&data[0]);  combinedStepsDesired = readUInt32LE(&data[4]);  oppositeDistance = (int32_t)readUInt32LE(&data[8]);  bearingAlignment = data[12];  desiredBearing100 = readUInt16LE(&data[13]);  actualBearing100 = readUInt16LE(&data[15]);  trackDir = data[17] & 0x01;
  Serial.println("RX: WAYPOINT DATA");  Serial.print(" pathTotalDistance=");  Serial.println(pathTotalDistance);  Serial.print(" combinedStepsDesired=");  Serial.println(combinedStepsDesired);  Serial.print(" oppositeDistance=");  Serial.println(oppositeDistance);  Serial.print(" bearingAlignment=");  Serial.println(bearingAlignment);  Serial.print(" desiredBearing100=");  Serial.println(desiredBearing100);  Serial.print(" actualBearing100=");  Serial.println(actualBearing100);  Serial.print(" trackDir=");  Serial.println(trackDir);
  sendNavAck();  beginForwardNavigation();}
void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {  switch (cmd) {    case CMD_STOP_MOTORS:      stopMotors();      sendTotalSteps(totalStepsTaken);      resetNavigationState();      break;
    case CMD_ACTUAL_BEARING:      if (len == 2) {        actualBearing100 = readUInt16LE(data);        Serial.print("RX: ACTUAL BEARING x100 = ");        Serial.println(actualBearing100);      }      break;
    case CMD_RIGHT_PIVOT:      if (len == 2 && !navRunning) {        // pivotRight(readUInt16LE(data));      }      break;
    case CMD_LEFT_PIVOT:      if (len == 2 && !navRunning) {        // pivotLeft(readUInt16LE(data));      }      break;
    case CMD_WAYPOINT_DATA:      if (!navRunning) handleWaypointPacket(data, len);      break;  }}
void processIncomingByte(uint8_t b) {  switch (parserState) {    case WAIT_HEADER:      if (b == UART_HEADER) parserState = WAIT_CMD;      break;
    case WAIT_CMD:      currentCmd = b;      runningChecksum = b;      parserState = WAIT_LEN;      break;
    case WAIT_LEN:      expectedLen = b;      runningChecksum ^= b;      if (expectedLen > MAX_PAYLOAD) {        parserState = WAIT_HEADER;      } else if (expectedLen == 0) {        parserState = WAIT_CHECKSUM;      } else {        payloadIndex = 0;        parserState = WAIT_PAYLOAD;      }      break;
    case WAIT_PAYLOAD:      payload[payloadIndex++] = b;      runningChecksum ^= b;      if (payloadIndex >= expectedLen) {        parserState = WAIT_CHECKSUM;      }      break;
    case WAIT_CHECKSUM:      if (b == runningChecksum) {        handlePacket(currentCmd, payload, expectedLen);      } else {        Serial.println("Checksum error");      }      parserState = WAIT_HEADER;      runningChecksum = 0;      currentCmd = 0;      expectedLen = 0;      payloadIndex = 0;      break;  }}
void readIncomingUart() {  while (Serial3.available() > 0) {    processIncomingByte((uint8_t)Serial3.read());  }}
void setup() {  Serial.begin(115200);  Serial3.begin(115200);
  LeftFrontWheel.setMaxSpeed(2000);  LeftBackWheel.setMaxSpeed(2000);  RightFrontWheel.setMaxSpeed(2000);  RightBackWheel.setMaxSpeed(2000);
  LeftFrontWheel.setAcceleration(800);  LeftBackWheel.setAcceleration(800);  RightFrontWheel.setAcceleration(800);  RightBackWheel.setAcceleration(800);
  multiStepper.addStepper(LeftFrontWheel);  multiStepper.addStepper(LeftBackWheel);  multiStepper.addStepper(RightFrontWheel);  multiStepper.addStepper(RightBackWheel);
  resetNavigationState();  Serial.println("ARDUINO_v6 ready");}
void runForwardLoop() {  computeDesiredBearing();  updateBearingDiff();
  // Your forward motion logic goes here}
void loop() {  readIncomingUart();  runForwardLoop();}

@Athena.

Attached is the latest Arduino sketch. Please rewrite the full Arduino_v7 sketch in a clean compile-ready form, including:

• your UART packet parser
• desiredBearing100-based side detection
• corrected bearing computation
• a proper forward motor execution loop
• safer stepper state handling
• clearer separation between:
  • navigation math
  • motion output
  • UART reporting

Thank you. Jack