Left motors stall until scan request

Question

@Athena - can you review the Arduino sketch below and explain why the left motors (LeftFront and LeftBack) do not run until sendObstacleScanRequest() has been called?

Context: the sketch communicates with ARC over Serial3 using a simple UART protocol (header 0xA5), so the Arduino is acting as the EZB / hardware controller receiving navigation/waypoint commands from ARC (for example, commands sent by a Robot Skill). The four motors are driven as steppers using AccelStepper and MultiStepper. I’ve included the full sketch below.

Thanks, Jack

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

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_PAYLOAD };
ParserState state = WAIT_HEADER;

uint8_t currentCmd = 0;
uint8_t payload[32];
uint8_t payloadIndex = 0;
uint8_t expectedLength = 0;

int RUN = 0;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
uint32_t oppositeDistance = 0;
uint8_t bearingAlignment = 0;
uint16_t reflectorBearing100 = 0;
uint8_t sensorPrime = 0;
uint16_t waypointDesiredBearing100 = 9000;
uint16_t waypointActualBearing100 = 9000;
uint8_t trackDir = 0;

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

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

bool obstacleScanRequested = false;
bool scanNeutralMode = false;
uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

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 writeUInt16LE(uint16_t value) {
  Serial3.write((uint8_t)(value & 0xFF));
  Serial3.write((uint8_t)((value >> 8) & 0xFF));
}

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

void sendHeaderAndCmd(uint8_t cmd) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
}

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

void sendTotalSteps(uint32_t steps) {
  sendHeaderAndCmd(RSP_TOTAL_STEPS);
  writeUInt32LE(steps);
  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  sendHeaderAndCmd(RSP_DESIRED_BEARING);
  writeUInt16LE(bearing100);
  lastSentDesiredBearing100 = bearing100;
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

void sendObstacleScanRequest() {
  sendHeaderAndCmd(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 resetNavigationState() {
  resetStepperPositions();
  totalStepsTaken = 0;
  desiredBearing100 = 9000;
  actualBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  RUN = 0;
  Serial.println("Navigation state reset");
  Serial.print("desiredBearing100:  ");
  Serial.println(desiredBearing100);
  Serial.print("totalStepsTaken:  ");
  Serial.println(totalStepsTaken);
  Serial.print("combinedStepsDesired:  ");
  Serial.println(combinedStepsDesired);
  Serial.print("pathTotalDistance:  ");
  Serial.println(pathTotalDistance);
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  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 pivotRight(uint16_t steps) {
  long positions[4];
  positions[0] = LeftFrontWheel.currentPosition() + steps;
  positions[1] = LeftBackWheel.currentPosition() + steps;
  positions[2] = RightFrontWheel.currentPosition() - steps;
  positions[3] = RightBackWheel.currentPosition() - steps;
  multiStepper.moveTo(positions);
  while (multiStepper.run()) { }
  totalStepsTaken += steps;
  stopMotors();
  sendTotalSteps(totalStepsTaken);
  resetNavigationState();
}

void pivotLeft(uint16_t steps) {
  long positions[4];
  positions[0] = LeftFrontWheel.currentPosition() - steps;
  positions[1] = LeftBackWheel.currentPosition() - steps;
  positions[2] = RightFrontWheel.currentPosition() + steps;
  positions[3] = RightBackWheel.currentPosition() + steps;
  multiStepper.moveTo(positions);
  while (multiStepper.run()) { }
  totalStepsTaken += steps;
  stopMotors();
  sendTotalSteps(totalStepsTaken);
  resetNavigationState();
}

void beginForwardNavigation() {
  RUN = 1;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

void computeDesiredBearing() {
  if (bearingAlignment == 0) {
    // desiredBearing100 = 9000;
    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;
  }
  float tangent = (float)oppositeDistance / (float)pathDistanceRemaining;
  float deg = atan(tangent) * 180.0 / PI;
  desiredBearing100 = (uint16_t)((deg + 90.0) * 100.0);
  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 runForwardLoop() {
  if (currentSpeed < normalSpeed) {
    currentSpeed += 10;
    if (currentSpeed > normalSpeed) currentSpeed = normalSpeed;
  }

  computeDesiredBearing();
  updateBearingDiff();

  int leftSpeed = currentSpeed;
  int rightSpeed = currentSpeed;

  if (!scanNeutralMode) {
    int correction = bearingDiffRamped / 10;
    leftSpeed -= correction;
    rightSpeed += correction;
  }

  setAllMotorSpeeds(leftSpeed, rightSpeed);

  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();

  totalStepsTaken =
    abs(LeftFrontWheel.currentPosition()) +
    abs(LeftBackWheel.currentPosition()) +
    abs(RightFrontWheel.currentPosition()) +
    abs(RightBackWheel.currentPosition()) / 4;

  // if (!obstacleScanRequested && totalStepsTaken >= nextScanStep) {
  //   sendObstacleScanRequest();
  //   obstacleScanRequested = true;
  //   scanNeutralMode = true;
  //   nextScanStep += scanIntervalSteps;
  // }

  if (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    scanNeutralMode = true;
    nextScanStep += scanIntervalSteps;
  }
  if (totalStepsTaken >= combinedStepsDesired && combinedStepsDesired > 0) {
    stopMotors();
    sendTotalSteps(totalStepsTaken);
    resetNavigationState();
  }
}

uint8_t getExpectedLength(uint8_t cmd) {
  switch (cmd) {
    case CMD_STOP_MOTORS:    return 0;
    case CMD_ACTUAL_BEARING: return 2;
    case CMD_RIGHT_PIVOT:    return 2;
    case CMD_LEFT_PIVOT:     return 2;
    case CMD_WAYPOINT_DATA:  return 18;
    default:                 return 0xFF;
  }
}

void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) return;

  pathTotalDistance = readUInt32LE(&data[0]);
  combinedStepsDesired = readUInt32LE(&data[4]);
  oppositeDistance = readUInt32LE(&data[8]);
  bearingAlignment = data[12];
  reflectorBearing100 = readUInt16LE(&data[13]);
  sensorPrime = data[15];
  waypointDesiredBearing100 = readUInt16LE(&data[16]);
  waypointActualBearing100 = actualBearing100;
  trackDir = 0;

  desiredBearing100 = waypointDesiredBearing100;
  actualBearing100 = waypointActualBearing100;

  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("actualBearing100:  ");
        Serial.println(actualBearing100);
      }
      break;
    case CMD_RIGHT_PIVOT:
      if (len == 2 && RUN == 0) pivotRight(readUInt16LE(data));
      break;
    case CMD_LEFT_PIVOT:
      if (len == 2 && RUN == 0) pivotLeft(readUInt16LE(data));
      break;
    case CMD_WAYPOINT_DATA:
      if (RUN == 0) handleWaypointPacket(data, len);
      break;
  }
}

void processIncomingByte(uint8_t b) {
  switch (state) {
    case WAIT_HEADER:
      if (b == UART_HEADER) state = WAIT_CMD;
      break;
    case WAIT_CMD:
      currentCmd = b;
      expectedLength = getExpectedLength(currentCmd);
      if (expectedLength == 0xFF) {
        state = WAIT_HEADER;
      } else if (expectedLength == 0) {
        handlePacket(currentCmd, nullptr, 0);
        state = WAIT_HEADER;
      } else {
        payloadIndex = 0;
        state = WAIT_PAYLOAD;
      }
      break;
    case WAIT_PAYLOAD:
      payload[payloadIndex++] = b;
      if (payloadIndex >= expectedLength) {
        handlePacket(currentCmd, payload, expectedLength);
        state = WAIT_HEADER;
      }
      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_v5 ready");
}

void loop() {
  readIncomingUart();
  if (RUN == 1) {
    runForwardLoop();
  }
}

Athena · Answer

Yes - the behavior is coming from the sketch logic, not ARC itself.

Short answer

Your left motors are being starved until sendObstacleScanRequest() runs because runForwardLoop() is the only place where motor speeds are continuously updated and runSpeed() is called.
Until that loop is actively running, the stepper drivers won’t be serviced often enough for AccelStepper to generate motion.

But there’s a second, more important issue:

Your code only enters forward-running mode after the waypoint packet is handled

beginForwardNavigation() sets:

RUN = 1
resets currentSpeed
resets scan flags

So the real question is: why do the left motors appear dead until the first obstacle scan request?

That points to the speed-correction logic and the scan-neutral behavior.

What I see in the sketch

1) Motor motion only happens inside `runForwardLoop()`

This block is critical:

if (RUN == 1) {
  runForwardLoop();
}

And inside runForwardLoop() you do all of this:

compute bearing
update speed
apply left/right correction
call setAllMotorSpeeds(leftSpeed, rightSpeed);
call runSpeed() on all 4 motors

So if RUN is not 1, nothing moves.

2) Your left motors are probably being driven to `0` or negative speed before the scan request

This section is suspicious:

int correction = bearingDiffRamped / 10;
leftSpeed -= correction;
rightSpeed += correction;
setAllMotorSpeeds(leftSpeed, rightSpeed);

If bearingDiffRamped is positive enough, the correction can push leftSpeed down to zero or below.

Because you’re using:

LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();

runSpeed() does not accelerate or recover a stalled motor if the speed is 0. It only steps when the speed is set properly.

So the left side may look "stalled" while the right side continues or the system waits for the scan event.

3) `scanNeutralMode` changes the correction behavior

Once this happens:

sendObstacleScanRequest();
scanNeutralMode = true;

then this block no longer applies correction:

if (!scanNeutralMode) {
  int correction = bearingDiffRamped / 10;
  leftSpeed -= correction;
  rightSpeed += correction;
}

That means both sides go back to the same currentSpeed, which makes the left motors suddenly start moving.

So the symptom fits this pattern:

before scan request: correction reduces left speed too much
after scan request: correction is disabled
left motors move again

There is also a bug in your step averaging line

This line has operator precedence trouble:

totalStepsTaken = abs(LeftFrontWheel.currentPosition()) + abs(LeftBackWheel.currentPosition()) + abs(RightFrontWheel.currentPosition()) + abs(RightBackWheel.currentPosition()) / 4;

Only the last term is divided by 4.

It should probably be:

totalStepsTaken =
  (abs(LeftFrontWheel.currentPosition()) +
   abs(LeftBackWheel.currentPosition()) +
   abs(RightFrontWheel.currentPosition()) +
   abs(RightBackWheel.currentPosition())) / 4;

If ARC is using totalStepsTaken to decide when scans happen, this bug could make scan timing very wrong.

Another issue: `currentSpeed` may never ramp up the way you expect

You do:

if (currentSpeed < normalSpeed) currentSpeed += 10;
if (currentSpeed > normalSpeed) currentSpeed = normalSpeed;

But I don’t see the full runForwardLoop() definition here, so if there’s any early return, or if currentSpeed is reset unexpectedly, the motors may never get enough speed to overcome static friction.

That often shows up more on one side than the other.

Most likely root cause

The most likely cause is:

Your left motors are being reduced to 0 or near-0 by the bearing correction logic until scanNeutralMode is enabled by sendObstacleScanRequest().

So the obstacle scan request is not fixing the motors - it’s disabling the correction that was suppressing the left side.

Add debug prints inside the loop right before setAllMotorSpeeds():

Serial.print("RUN=");
Serial.print(RUN);
Serial.print(" currentSpeed=");
Serial.print(currentSpeed);
Serial.print(" bearingDiffRamped=");
Serial.print(bearingDiffRamped);
Serial.print(" leftSpeed=");
Serial.print(leftSpeed);
Serial.print(" rightSpeed=");
Serial.print(rightSpeed);
Serial.print(" scanNeutralMode=");
Serial.println(scanNeutralMode);

If you see leftSpeed <= 0 before the scan request, that confirms it.

Safer fix

Clamp the speeds before sending them to the steppers:

leftSpeed = constrain(leftSpeed, 0, normalSpeed);
rightSpeed = constrain(rightSpeed, 0, normalSpeed);
setAllMotorSpeeds(leftSpeed, rightSpeed);

If you need reverse capability, handle negative values intentionally instead of feeding them directly to runSpeed().

One more thing to verify

Make sure your runForwardLoop() is actually being called continuously and not only after a scan event. If you can paste that function, I can review it line by line - that’s probably where the exact left-side stall is happening.

Relevant ARC note

Since this is acting like a hardware controller for ARC, your setup fits the kind of microcontroller offload ARC is designed for: https://synthiam.com/Support/ARC-Overview/arc-and-micros-for-closed-loop-functions

If you want, I can also help you:

rewrite this into cleaner Arduino/ARC UART protocol code
add speed clamping and motor debug telemetry
or convert the logic into a more robust state machine for waypoint + scan handling

If you paste runForwardLoop() and the exact serial packet flow from ARC, I’ll pinpoint the fault more precisely.

I am a customer support GPT-powered AI and still learning. If you have doubts about my response, please use the support documentation to verify my answer.

Robojack

Left Motors Stall Until Scan Request

Upgrade to ARC Pro

Short answer

Your code only enters forward-running mode after the waypoint packet is handled

What I see in the sketch

1) Motor motion only happens inside `runForwardLoop()`

2) Your left motors are probably being driven to `0` or negative speed before the scan request

3) `scanNeutralMode` changes the correction behavior

There is also a bug in your step averaging line

Another issue: `currentSpeed` may never ramp up the way you expect

Most likely root cause

Safer fix

One more thing to verify

Relevant ARC note

Products

Community

Support

About

Robojack

Left Motors Stall Until Scan Request

Upgrade to ARC Pro

Short answer

Your code only enters forward-running mode after the waypoint packet is handled

What I see in the sketch

1) Motor motion only happens inside runForwardLoop()

2) Your left motors are probably being driven to 0 or negative speed before the scan request

3) scanNeutralMode changes the correction behavior

There is also a bug in your step averaging line

Another issue: currentSpeed may never ramp up the way you expect

Most likely root cause

What I recommend checking first

Safer fix

One more thing to verify

Relevant ARC note

1) Motor motion only happens inside `runForwardLoop()`

2) Your left motors are probably being driven to `0` or negative speed before the scan request

3) `scanNeutralMode` changes the correction behavior

Another issue: `currentSpeed` may never ramp up the way you expect