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.