Text Overlay icon Text Overlay — Overlay a variable on a processed camera image at X/Y coordinates; name the variable and attach to a specific or any available camera Try it →
USA
YouTube
Asked

Stepper Reset Issue In Arduino Sketch

Skip to comments   Jump to end

I'm using an Arduino Mega as the hardware controller (acting like an EZB/microcontroller) to provide PWM to four stepper motors via the AccelStepper and MultiStepper libraries. The sketch uses Serial3 (Serial3.begin(115200)) for UART communication, so this Arduino would communicate with a PC or Synthiam ARC over that serial link if connected to an ARC project.

Problem: after the motors run the predetermined number of steps, I cannot reset the PWM/step count so the motor-run sequence can restart. The Arduino seems to think it has already completed the predetermined number of steps and never re-runs. Only when I upload the program again into the Arduino does it run the predetermined number of steps.

Can you please look at this sketch and troubleshoot it? Thanks.

Jack

#include <AccelStepper.h>#include <MultiStepper.h>
static const uint8_t UART_HEADER = 0xA5;
// ARC -> Arduinostatic 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;
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 resetNavigationState() {  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());
  if (!obstacleScanRequested && totalStepsTaken >= nextScanStep) {    sendObstacleScanRequest();    obstacleScanRequested = true;    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);      }      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();  }}

Related Hardware (view all EZB hardware)
Arduino Due/Mega
Arduino Due/Mega by Arduino
Synthiam ARC enables Arduino Mega 2560 control with 8 ADC, 13 PWM, 21 digital ports and 24 servos; includes firmware, shield and tutorials
USB
Servos 24
Camera
Audio
UART 3
I2C
ADC 8
Digital 21

Related Robot Skills (view all robot skills)
Thumbnail
Script by Synthiam
Multi-language ARC Script: build, run and debug Blockly, JavaScript, EZ-Script or Python with Intellisense, run/save/load and Roboscratch support
Requires ARC
Scripting
Thumbnail
EZ-Script Console by Synthiam
Real-time EZ-Script console for ARC: enter and execute single-line commands, view output, clear display, and recall history with UP/DOWN.
Requires ARC
Scripting
Jump to end

ARC Pro

Upgrade to ARC Pro

Your robot can be more than a simple automated machine with the power of ARC Pro!

Author Avatar
PRO
Canada
Twitter YouTube GitHub
#1  

Yes - the issue is very likely state management, not the stepper libraries themselves.

What I see in your sketch

A few things stand out:

1. resetNavigationState() does not reset the stepper positions

You reset your navigation variables like:

  • totalStepsTaken
  • desiredBearing100
  • actualBearing100
  • RUN

But you never reset the internal stepper position counters:

  • LeftFrontWheel.currentPosition()
  • LeftBackWheel.currentPosition()
  • RightFrontWheel.currentPosition()
  • RightBackWheel.currentPosition()

That matters because your forward loop does this:

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

So even if you set totalStepsTaken = 0, the next loop immediately overwrites it with the old step counts if the steppers still think they’re at their previous positions.

2. stopMotors() does not reset the step counters either

stop() only stops motion. It does not zero the encoder/position state of AccelStepper.

If you want a fresh run, you need to explicitly zero the position with:

setCurrentPosition(0)

for each stepper.

3. Your "done" condition can re-trigger immediately

You have:

if (totalStepsTaken >= combinedStepsDesired && combinedStepsDesired > 0) {
  stopMotors();
  sendTotalSteps(totalStepsTaken);
  resetNavigationState();
}

If the internal current positions still hold the previous run’s values, then your next run may instantly think it already finished.

The fix

Add a function to zero all four steppers:

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

Then call it inside resetNavigationState():

void resetNavigationState() {
  stopMotors();

  resetStepperPositions();

  totalStepsTaken = 0;
  combinedStepsDesired = 0;
  pathTotalDistance = 0;
  oppositeDistance = 0;
  bearingAlignment = 0;
  reflectorBearing100 = 0;
  sensorPrime = 0;
  waypointDesiredBearing100 = 9000;
  waypointActualBearing100 = 9000;
  trackDir = 0;
  actualBearing100 = 9000;
  desiredBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  obstacleScanRequested = false;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  RUN = 0;

  Serial.println("Navigation state reset");
}

Important note about MultiStepper

If you’re using multiStepper.moveTo(positions); then the library is working from target positions relative to the current internal stepper positions.

If you reset only your own variables but not the steppers, the library still uses stale internal coordinates.

So the reset must include:

setCurrentPosition(0)

before the next movement cycle starts.

Also check this logic

You’re using:

totalStepsTaken = abs(LeftFrontWheel.currentPosition()) + ...

That is unusual for a stepper system unless you intentionally want cumulative absolute distance across all motors.

If one motor goes negative and another positive, absolute sum may not represent actual path progress the way you expect.

If your goal is "how far has the robot progressed," it may be better to track a separate runtime counter that you increment as commands are sent, instead of deriving it from stepper position each loop.

One more possible bug

This function appears malformed in the pasted code:

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

It should likely be:

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

If that typo exists in the actual sketch, it will break packet parsing.

Suggested minimal change

If you want the smallest possible fix, try this first:

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

  totalStepsTaken = 0;
  combinedStepsDesired = 0;
  RUN = 0;

  Serial.println("Navigation state reset");
}

ARC-side note

If ARC is sending the "start route" packet again, make sure your Arduino actually receives a new CMD_WAYPOINT_DATA packet after reset. If not, the Arduino will just sit in RUN == 0 waiting.

My recommendation

  1. Zero all stepper positions with setCurrentPosition(0)
  2. Reset combinedStepsDesired, pathTotalDistance, and any other route-specific state
  3. Confirm the PC/ARC is actually sending a fresh waypoint packet after reset
  4. Fix the readUInt16LE() typo if it exists in the real code

If you want, I can help you do a clean rewrite of the reset/start state machine so the Arduino can reliably:

  • receive a route
  • run it
  • stop
  • reset
  • run it again

If you’d like, paste the actual runForwardLoop() function too - that’s probably where the restart logic should be tightened up.


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.
Login to post a comment