Roomba Tones icon Roomba Tones — Deprecated Roomba control; replaced by the iRobot Roomba Movement Panel for updated movement control. Try it →
USA
YouTube
Asked

Obstacle Scan Only Once

Skip to comments   Jump to end

,

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

This is an Arduino sketch that communicates over Serial3 using a simple UART protocol (header 0xA5). It should trigger sendObstacleScanRequest() every 10,500 steps, but it only seems to occur at the first 10,500-step mark and not subsequently. Is this line of code correct?

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

For reference, Serial3 is the serial port used to communicate with ARC (or a host controller/EZB) in this setup.

Thanks, Jack


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
NMS Navigation Data Grabber by Synthiam Inc.
Query NMS lidar scan data by angle, with control commands for range, arc extremes, and obstacle checks from the latest scan.
Requires ARC v2
Navigation
Jump to end

ARC Pro

Upgrade to ARC Pro

With ARC Pro, your robot is not just a machine; it's your creative partner in the journey of technological exploration.

Author Avatar
PRO
Canada
Twitter YouTube GitHub
#1  

Yes - the issue is very likely that obstacleScanRequested is never being cleared back to false after the first request.

Right now your logic does this:

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

That means:

  1. At 10,500 steps, it sends the request
  2. obstacleScanRequested becomes true
  3. On the next loop, the condition can never pass again because you never set it back to false

Why it only happens once

Your code increments nextScanStep, but the !obstacleScanRequested gate blocks future requests forever.

Fix options

Option 1: Reset the flag when the scan response is received

If ARC or your host sends a response back after the scan, that response handler should clear the flag:

obstacleScanRequested = false;
scanNeutralMode = false;

That would allow the next 10,500-step trigger to fire.

Option 2: Remove the flag entirely

If you only want one request per interval, the nextScanStep check is enough by itself:

if (totalStepsTaken >= nextScanStep) {
  sendObstacleScanRequest();
  scanNeutralMode = true;
  nextScanStep += scanIntervalSteps;
}

This works only if runForwardLoop() runs often enough and you don’t need a "wait for response" lockout.

Option 3: Use a response/timeout state machine

A safer approach is:

  • send request
  • set obstacleScanRequested = true
  • wait for reply
  • when reply arrives, clear obstacleScanRequested
  • optionally start a timeout fallback in case no reply arrives

That’s usually the best pattern for serial robot navigation.

One more thing to check

You calculate:

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

If your steppers are not all moving the way you expect, this could jump in unexpected ways. But that would not explain "only once" as clearly as the boolean flag does.

If you want repeated obstacle scan requests every 10,500 steps, I’d start with this:

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

Then, wherever you process the scan reply:

obstacleScanRequested = false;
scanNeutralMode = false;

ARC side note

If this Arduino is talking to ARC over Serial3, make sure your ARC side skill is reading the custom protocol consistently. In ARC, the best place to handle this is usually a Script skill using JavaScript or Python, or a custom robot skill if you’re building a reusable interface.

If you want, I can help you turn this into a cleaner state machine so the scan request/reply cycle is reliable and easier to debug.

Also, if you’d like, I can review the runForwardLoop() portion too - there may be a second issue there affecting the repeat trigger.


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