Robot Skill To Camera icon Robot Skill To Camera Capture a robot skill's display and stream selected area as video to a configured camera device with FPS and live preview crop. Try it →
Asked — Edited
Resolved Resolved by Athena!

Distance Sensor Query

@Athena,

Thread theme: Scanning strategy

Since IR_TRACKER_v1 and OBS_SCAN_v1 share the same servo and the same IR sensor and make incompatible assumptions about the sensor values, they should not run at the same time. These are both Robot Skills in my ARC project, so my intended method is:

  • Let IR_TRACKER_v1 own the main loop (it will be the active robot skill controlling the IR sensor most of the time).
  • Have IR_TRACKER_v1 check for $obsScanRequested == 1.
  • Pause the tracker logic when a scan is requested.
  • Start OBS_SCAN_v1 with controlCommand("OBS_SCAN_v1", "ScriptStartWait").
  • Resume IR_TRACKER_v1 after the scanning script completes.

I think this is a clean design because it provides:

  • One robot skill in control of the IR sensor at a time.
  • No conflicting servo motion.
  • Predictable scan behavior.
  • Easier debugging.

For the initial scan before movement, I have ROVER_CMD_v5 start OBS_SCAN_v1 by inserting it into the startup order along with UART_v7, LEG_v5, and IR_TRACKER_v1. All flags start at 0:

  • setVar("$waypointSent", 0);
  • setVar("$sendWaypointNow", 0);
  • setVar("$obsScanRequested", 0);
  • etc.

Startup sequence (ROVER_CMD_v5):

  • FIRST START: controlCommand("UART_v7", "ScriptStart") - UART is running; flags are 0. The UART script loops waiting for flag changes.
  • SECOND START: controlCommand("OBS_SCAN_v1", "ScriptStartWait") - OBS_SCAN_v1 completes the obstacle scan and either sets a global variable $obsDet = 1 or times out and issues a global variable to STOP. UART picks this up and the rover is shut down if needed.
  • THIRD START: controlCommand("LEG_v5", "ScriptStartWait") - LEG_v5 populates all waypoint data global variables and sets "$sendWaypointNow = 1". The UART loop detects this flag and sends WAYPOINT DATA to the Arduino.
  • FOURTH START: controlCommand("IR_TRACKER_v1", "ScriptStart") - from this point forward, the only flag toggled for obstacle scanning will be "$obsScanRequested". It is initially 0 and is set to 1 when the Arduino counts steps to the first scan interval.

ROVER_CMD_v5 (current script):

// ROVER_CMD_v5

controlCommand("UART_v7.1", "ScriptStart");

controlCommand("OBS_SCAN_v1", "ScriptStartWait");

// Load the leg values
controlCommand("LEG_v5", "ScriptStartWait");//scriptstartwait

// At this point LEG_v5 will have queued the waypoint transmission
print("ROVER_CMD_v5: leg loaded and UART running");
controlCommand("IR_TRACKER_v1", "ScriptStart");

What do you think of this strategy for the initial obstacle scan? The remaining obstacle scans will be handled by IR_TRACKER_v1 to avoid conflicts with the IR sensor data.

Note: It appears the Arduino is connected to ARC via a UART/serial robot skill (UART_v7) running on the PC. Also, there is a small naming ambiguity in the scripts: earlier I refer to $obsScanRequested but the UART_v7 handler sets $obstacleScanRequested on receiving an RSP_OBS_SCAN_REQUEST. I’m calling this out so we can ensure the global variable names match across Robot Skills.

If you agree with the strategy above, can you rewrite an updated Arduino_v7 that will work with the latest UART_v7 posted below?

Thank you, Jack

UART_v7

var UART_INDEX = 0;

var UART_HEADER = 0xA5;

var CMD_STOP_MOTORS = 0x01;
var CMD_ACTUAL_BEARING = 0x03;
var CMD_RIGHT_PIVOT = 0x04;
var CMD_LEFT_PIVOT = 0x05;
var CMD_WAYPOINT_DATA = 0x06;

var RSP_NAV_ACK = 0x10;
var RSP_TOTAL_STEPS = 0x11;
var RSP_DESIRED_BEARING = 0x12;
var RSP_OBS_SCAN_REQUEST = 0x13;

var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

function safeInt(v, def) {
  var n = parseInt(v);
  if (isNaN(n)) return def;
  return n;
}

function clamp(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

function u16ToBytes(value) {
  value = value & 0xFFFF;
  return [value & 0xFF, (value >> 8) & 0xFF];
}

function u32ToBytes(value) {
  value = value >>> 0;
  return [
    value & 0xFF,
    (value >> 8) & 0xFF,
    (value >> 16) & 0xFF,
    (value >> 24) & 0xFF
  ];
}

function bytesToU16LE(b0, b1) {
  return (b0 & 0xFF) | ((b1 & 0xFF) << 8);
}

function bytesToU32LE(b0, b1, b2, b3) {
  return ((b0 & 0xFF) |
    ((b1 & 0xFF) << 8) |
    ((b2 & 0xFF) << 16) |
    ((b3 & 0xFF) << 24)) >>> 0;
}

function checksumXor(cmd, len, payload) {
  var c = (cmd ^ len) & 0xFF;
  for (var i = 0; i < payload.length; i++) {
    c ^= (payload[i] & 0xFF);
  }
  return c & 0xFF;
}

function sendPacket(cmd, payload) {
  var checksum = checksumXor(cmd, payload.length, payload);
  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER, cmd, payload.length]);
  if (payload.length > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
  }
  UART.hardwareUartWrite(UART_INDEX, [checksum]);
}

function sendStopMotors() {
  sendPacket(CMD_STOP_MOTORS, []);
  print("TX: STOP MOTORS");
}

function sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);

  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;

  var desiredBearing100 = clamp(safeInt(getVar("$desiredBearing100", 9000), 9000), 0, 18000);
  var actualBearing100 = clamp(safeInt(getVar("$actualBearing100", 9000), 9000), 0, 18000);
  var scanIntervalSteps = safeInt(getVar("$scanIntervalSteps", 3056), 3056) >>> 0;

  setVar("$desiredBearing100", desiredBearing100);
  setVar("$actualBearing100", actualBearing100);

  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload = payload.concat(u16ToBytes(actualBearing100));
  payload.push(trackDir);
  payload = payload.concat(u32ToBytes(scanIntervalSteps));

  if (payload.length !== 22) {
    print("Waypoint payload length error: " + payload.length);
    return;
  }

  sendPacket(CMD_WAYPOINT_DATA, payload);

  print("TX: WAYPOINT DATA");
  print(" pathTotalDistance=" + pathTotalDistance);
  print(" combinedStepsDesired=" + combinedStepsDesired);
  print(" oppositeDistance=" + oppositeDistance);
  print(" bearingAlignment=" + bearingAlignment);
  print(" desiredBearing100=" + desiredBearing100);
  print(" actualBearing100=" + actualBearing100);
  print(" trackDir=" + trackDir);
  print(" scanIntervalSteps=" + scanIntervalSteps);
}

function sendRightPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_RIGHT_PIVOT, u16ToBytes(steps));
  print("TX: RIGHT PIVOT steps=" + steps);
}

function sendLeftPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_LEFT_PIVOT, u16ToBytes(steps));
  print("TX: LEFT PIVOT steps=" + steps);
}

function handleRxPacket(cmd, payload) {
  if (cmd == RSP_NAV_ACK) {
    print("RX: NAV ACK");
    setVar("$navAckReceived", 1);
    return;
  }

  if (cmd == RSP_TOTAL_STEPS) {
    if (payload.length == 4) {
      var steps = bytesToU32LE(payload[0], payload[1], payload[2], payload[3]);
      setVar("$totalStepsTaken", steps);
      setVar("$legComplete", 1);
      print("RX: TOTAL STEPS = " + steps);
    }
    return;
  }

  if (cmd == RSP_DESIRED_BEARING) {
    if (payload.length == 2) {
      var bearing100 = bytesToU16LE(payload[0], payload[1]);
      print("RX: DESIRED BEARING x100 = " + bearing100);
      setVar("$desiredBearing100", bearing100);
    }
    return;
  }

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    print("RX: OBSTACLE SCAN REQUEST");
    setVar("$obstacleScanRequested", 1);
    return;
  }

  print("RX: Unknown cmd=" + cmd + " len=" + payload.length);
}

function processIncomingBytes() {
  var available = UART.hardwareUartAvailable(UART_INDEX);

  while (available > 0) {
    var b = UART.hardwareUartRead(UART_INDEX, 1)[0] & 0xFF;
    available--;

    if (rxState == 0) {
      if (b == UART_HEADER) rxState = 1;
      continue;
    }

    if (rxState == 1) {
      rxCmd = b;
      rxChecksum = rxCmd;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b;
      rxChecksum ^= rxLen;
      rxPayload = [];
      rxIndex = 0;
      rxState = (rxLen == 0) ? 4 : 3;
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b);
      rxChecksum ^= b;
      rxIndex++;
      if (rxIndex >= rxLen) rxState = 4;
      continue;
    }

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        handleRxPacket(rxCmd, rxPayload);
      } else {
        print("RX checksum error. cmd=" + rxCmd + " expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

      rxState = 0;
      rxCmd = 0;
      rxLen = 0;
      rxIndex = 0;
      rxPayload = [];
      rxChecksum = 0;
      continue;
    }
  }
}

print("UART_v7 started");

while (true) {
  if (getVar("$sendWaypointNow", 0) == 1 && getVar("$waypointSent", 0) == 0) {
    sendWaypointData();
    print("$sendWaypointNow:  " + getVar("$sendWaypointNow",0));
    setVar("$sendWaypointNow", 0);
    setVar("$waypointSent", 1);

  }
  if (getVar("$sendStopNow", 0) == 1) {
    sendStopMotors();
    setVar("$sendStopNow", 0);
  }

  if (getVar("$sendActualBearingNow", 0) == 1) {
    sendActualBearing();
    setVar("$sendActualBearingNow", 0);
  }

  if (getVar("$sendRightPivotNow", 0) == 1) {
    sendRightPivot();
    setVar("$sendRightPivotNow", 0);
  }

  if (getVar("$sendLeftPivotNow", 0) == 1) {
    sendLeftPivot();
    setVar("$sendLeftPivotNow", 0);
  }

  processIncomingBytes();
  sleep(20);
}


Related Hardware (view all EZB hardware)
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

ARC Pro

Upgrade to ARC Pro

With Synthiam ARC Pro, you're not just programming a robot; you're shaping the future of automation, one innovative idea at a time.

#1  

@Athena.

Note: It appears the Arduino is connected to ARC via a UART/serial robot skill (UART_v7) running on the PC. Also, there is a small naming ambiguity in the scripts: earlier I refer to $obsScanRequested but the UART_v7 handler sets $obstacleScanRequested on receiving an RSP_OBS_SCAN_REQUEST. I’m calling this out so we can ensure the global variable names match across Robot Skills.

Thank you for this note. Any "$obstacleScanRequested" should be named "$obsScanRequested" . Thank you Jack

Author Avatar
PRO
Canada
Twitter YouTube GitHub
#2  

Yes - your strategy is solid.

Having one skill own the IR sensor and shared servo at a time is the cleanest way to avoid:

  • conflicting servo commands
  • inconsistent IR readings
  • race conditions between scripts
  • hard-to-debug "who moved the sensor last?" problems

Your plan makes sense:

  1. ROVER_CMD_v5 starts UART_v7
  2. ROVER_CMD_v5 runs OBS_SCAN_v1 once at startup with ScriptStartWait
  3. ROVER_CMD_v5 runs LEG_v5 with ScriptStartWait
  4. ROVER_CMD_v5 starts IR_TRACKER_v1
  5. Later obstacle scans are requested by setting $obsScanRequested = 1
  6. IR_TRACKER_v1 pauses its own logic, runs OBS_SCAN_v1, then resumes

That’s a good ARC design because it uses robot skill messaging cleanly, and ARC skills are meant to cooperate that way through globals and controlCommand().

Important naming fix

Yes, you should standardize on:

  • $obsScanRequested

and not:

  • $obstacleScanRequested

If UART_v7 is setting the longer variable name but the other skills are checking the shorter one, the request will never be seen.

So in UART_v7, change:

setVar("$obstacleScanRequested", 1);

to:

setVar("$obsScanRequested", 1);

One thing I would improve

For the scan handoff, I’d recommend also using a "scan busy" flag so both skills stay synchronized:

  • $obsScanRequested = 1 tracker requests a scan
  • $obsScanActive = 1 scan skill is currently running
  • $obsScanRequested = 0 when the request is accepted
  • $obsScanActive = 0 when the scan finishes

That prevents repeated requests while the scan is already in progress.

Updated ROVER_CMD_v5

Your startup script should be a little safer like this:

controlCommand("UART_v7", "ScriptStart");
controlCommand("OBS_SCAN_v1", "ScriptStartWait");
controlCommand("LEG_v5", "ScriptStartWait");

print("ROVER_CMD_v5: leg loaded and UART running");

controlCommand("IR_TRACKER_v1", "ScriptStart");

If you want the initial scan to happen before the leg data is sent, that order is correct.

Suggested behavior inside IR_TRACKER_v1

When it sees a scan request:

if (getVar("$obsScanRequested", 0) == 1) {
  setVar("$obsScanActive", 1);
  setVar("$obsScanRequested", 0);

  controlCommand("OBS_SCAN_v1", "ScriptStartWait");

  setVar("$obsScanActive", 0);
}

That gives you a clean pause/resume model.

Updated UART_v7 fix

Here is the exact change you need in the receive handler:

if (cmd == RSP_OBS_SCAN_REQUEST) {
  print("RX: OBSTACLE SCAN REQUEST");
  setVar("$obsScanRequested", 1);
  return;
}

My recommendation

I would not have both IR_TRACKER_v1 and OBS_SCAN_v1 directly manipulating the shared sensor at the same time. Your plan to serialize access is the right approach in ARC.

If you want, I can rewrite your full UART_v7 with:

  • the variable-name fix
  • a safer scan-state handshake
  • optional obsScanActive support
  • small cleanup for consistency

If you paste OBS_SCAN_v1 and IR_TRACKER_v1, I can also help you make the handoff logic match perfectly.

I am a customer support AI and still learning. Please verify important answers in our support documentation. For advanced AI features, use the AI Script Agent built into ARC.
#3  

@Athena. Thank you, excellent recommendations.

  • For the scan handoff, I’d recommend also using a "scan busy" flag so both skills stay synchronized:

    • $obsScanRequested = 1 tracker requests a scan
    • $obsScanActive = 1 scan skill is currently running
    • $obsScanRequested = 0 when the request is accepted
    • $obsScanActive = 0 when the scan finishes

    That prevents repeated requests while the scan is already in progress.

  • Inside IR_TRACKER_v1:

if (getVar("$obsScanRequested", 0) == 1) {
  setVar("$obsScanActive", 1);
  setVar("$obsScanRequested", 0);

  controlCommand("OBS_SCAN_v1", "ScriptStartWait");

  setVar("$obsScanActive", 0);
  • Inside ROVER_CMD_v5:
controlCommand("UART_v7", "ScriptStart");
controlCommand("OBS_SCAN_v1", "ScriptStartWait");
controlCommand("LEG_v5", "ScriptStartWait");

print("ROVER_CMD_v5: leg loaded and UART running");

controlCommand("IR_TRACKER_v1", "ScriptStart");
  • This sequence is intentional because when LEG_v5 runs, UART will send the globals as WAYPOINT DATA and motors will operate. OBS_SAN_v1 starts before WAYPOINT DATA is populated.

Can you rewrite UART_v7 with the following corrections and label it UART_v8?

  • the variable-name fix
  • a safer scan-state handshake
  • optional obsScanActive support
  • small cleanup for consistency

Also, Arduino_v7 is posted below. Can you rewrite it to seamlessly work with the new UART_v8? Thanks. Jack

Arduino_v7

#include <AccelStepper.h>#include <MultiStepper.h>#include <math.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;
// -------------------------// Motor setup// -------------------------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;
// -------------------------// Parser// -------------------------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;
// -------------------------// Navigation state// -------------------------bool navRunning = false;bool scanNeutralMode = false;
uint32_t totalStepsTaken = 0;uint32_t combinedStepsDesired = 0;uint32_t pathTotalDistance = 0;int32_t oppositeDistance = 0;
uint32_t nextScanStep = 0;uint32_t scanIntervalSteps = 3056; // ~24 inches based on 200 step motor, 1/8 microstepping, 4" wheel
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;
// -------------------------// Helpers// -------------------------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 sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {  uint8_t checksum = cmd ^ len;  Serial3.write(UART_HEADER);  Serial3.write(cmd);  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {    Serial3.write(data[i]);    checksum ^= data[i];  }
  Serial3.write(checksum);}
void sendCmdOnly(uint8_t cmd) {  sendPacket(cmd, nullptr, 0);}
void sendNavAck() {  sendCmdOnly(RSP_NAV_ACK);  Serial.println(F("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(F("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(F("TX: DESIRED BEARING x100 = "));  Serial.println(bearing100);}
void sendObstacleScanRequest() {  sendCmdOnly(RSP_OBS_SCAN_REQUEST);  Serial.println(F("TX: OBSTACLE SCAN REQUEST"));}
void resetStepperPositions() {  LeftFrontWheel.setCurrentPosition(0);  LeftBackWheel.setCurrentPosition(0);  RightFrontWheel.setCurrentPosition(0);  RightBackWheel.setCurrentPosition(0);}
uint32_t getAverageAbsStepperPosition() {  uint32_t a = (uint32_t)labs(LeftFrontWheel.currentPosition());  uint32_t b = (uint32_t)labs(LeftBackWheel.currentPosition());  uint32_t c = (uint32_t)labs(RightFrontWheel.currentPosition());  uint32_t d = (uint32_t)labs(RightBackWheel.currentPosition());
  return (a + b + c + d) / 4;}
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(F("Action: stopMotors()"));}
void resetNavigationState() {  resetStepperPositions();
  totalStepsTaken = 0;  desiredBearing100 = 9000;  actualBearing100 = 9000;  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;  bearingDiffRamped = 0;
  scanNeutralMode = false;  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;  navRunning = false;
  Serial.println(F("Navigation state reset"));}
void beginForwardNavigation() {  navRunning = true;  currentSpeed = 0;  bearingDiffRamped = 0;  scanNeutralMode = false;  nextScanStep = scanIntervalSteps;  Serial.println(F("Navigation started"));}
// -------------------------// Navigation math// -------------------------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;  }
  // 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);  }}
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 applyForwardMotion() {  int absDiff = abs(bearingDiffRamped);
  int leftSpeed = normalSpeed;  int rightSpeed = normalSpeed;
  if (bearingDiffRamped > 0) {    // actual > desired    // slow left side, speed right side    leftSpeed = normalSpeed - (absDiff * 2);    rightSpeed = normalSpeed + (absDiff * 2);  } else if (bearingDiffRamped < 0) {    // actual < desired    // slow right side, speed left side    leftSpeed = normalSpeed + (absDiff * 2);    rightSpeed = normalSpeed - (absDiff * 2);  }
  leftSpeed = constrain(leftSpeed, 0, 2000);  rightSpeed = constrain(rightSpeed, 0, 2000);
  setAllMotorSpeeds(leftSpeed, rightSpeed);}
void updateObstacleScanSchedule() {  if (!navRunning) {    return;  }
  while (totalStepsTaken >= nextScanStep) {    sendObstacleScanRequest();    nextScanStep += scanIntervalSteps;  }}
void runForwardLoop() {  if (!navRunning) {    return;  }
  computeDesiredBearing();  updateBearingDiff();  applyForwardMotion();
  LeftFrontWheel.runSpeed();  LeftBackWheel.runSpeed();  RightFrontWheel.runSpeed();  RightBackWheel.runSpeed();
  totalStepsTaken = getAverageAbsStepperPosition();
  updateObstacleScanSchedule();
  if (combinedStepsDesired > 0 && totalStepsTaken >= combinedStepsDesired) {    stopMotors();    sendTotalSteps(totalStepsTaken);    resetNavigationState();  }}
// -------------------------// Packet handlers// -------------------------void handleWaypointPacket(const uint8_t* data, uint8_t len) {  if (len != 18) {    Serial.println(F("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(F("RX: WAYPOINT DATA"));  Serial.print(F(" pathTotalDistance="));  Serial.println(pathTotalDistance);  Serial.print(F(" combinedStepsDesired="));  Serial.println(combinedStepsDesired);  Serial.print(F(" oppositeDistance="));  Serial.println(oppositeDistance);  Serial.print(F(" bearingAlignment="));  Serial.println(bearingAlignment);  Serial.print(F(" desiredBearing100="));  Serial.println(desiredBearing100);  Serial.print(F(" actualBearing100="));  Serial.println(actualBearing100);  Serial.print(F(" 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(F("RX: ACTUAL BEARING x100 = "));        Serial.println(actualBearing100);      }      break;
    case CMD_RIGHT_PIVOT:      if (len == 2 && !navRunning) {        Serial.println(F("RX: RIGHT PIVOT"));      }      break;
    case CMD_LEFT_PIVOT:      if (len == 2 && !navRunning) {        Serial.println(F("RX: LEFT PIVOT"));      }      break;
    case CMD_WAYPOINT_DATA:      if (!navRunning) {        handleWaypointPacket(data, len);      }      break;  }}
// -------------------------// UART parser// -------------------------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(F("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());  }}
// -------------------------// Arduino entry points// -------------------------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);
  LeftFrontWheel.setSpeed(0);  LeftBackWheel.setSpeed(0);  RightFrontWheel.setSpeed(0);  RightBackWheel.setSpeed(0);
  multiStepper.addStepper(LeftFrontWheel);  multiStepper.addStepper(LeftBackWheel);  multiStepper.addStepper(RightFrontWheel);  multiStepper.addStepper(RightBackWheel);
  resetNavigationState();
  Serial.println(F("ARDUINO_v7 ready"));}
void loop() {  readIncomingUart();  runForwardLoop();}
Author Avatar
PRO
Canada
Twitter YouTube GitHub
#4  

Absolutely - this is a good time to cleanly formalize the protocol.

Your scan handshake idea is solid, and I agree with your naming correction:

  • use $obsScanRequested
  • optionally use $obsScanActive
  • keep one skill responsible for the shared IR sensor/servo at a time

That will make the ARC side much easier to reason about.

Below is a rewritten UART_v8 for ARC and a compatible Arduino_v8 that matches it.


What changed in UART_v8

Fixes and improvements

  • Renamed scan request variable to $obsScanRequested
  • Added $obsScanActive support
  • Added a small handshake helper so the scan request is only latched if a scan is not already active
  • Cleaned up parsing/utility naming for consistency
  • Kept your existing packet structure compatible

UART_v8 for ARC

var UART_INDEX = 0;
var UART_HEADER = 0xA5;

// ARC -> Arduino
var CMD_STOP_MOTORS = 0x01;
var CMD_ACTUAL_BEARING = 0x03;
var CMD_RIGHT_PIVOT = 0x04;
var CMD_LEFT_PIVOT = 0x05;
var CMD_WAYPOINT_DATA = 0x06;

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

var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

function safeInt(v, def) {
  var n = parseInt(v);
  if (isNaN(n)) return def;
  return n;
}

function clamp(v, min, max) {
  if (v < min) return min;
  if (v > max) return max;
  return v;
}

function u16ToBytes(value) {
  value = value & 0xFFFF;
  return [value & 0xFF, (value >> 8) & 0xFF];
}

function u32ToBytes(value) {
  value = value >>> 0;
  return [
    value & 0xFF,
    (value >> 8) & 0xFF,
    (value >> 16) & 0xFF,
    (value >> 24) & 0xFF
  ];
}

function bytesToU16LE(b0, b1) {
  return (b0 & 0xFF) | ((b1 & 0xFF) << 8);
}

function bytesToU32LE(b0, b1, b2, b3) {
  return (
    (b0 & 0xFF) |
    ((b1 & 0xFF) << 8) |
    ((b2 & 0xFF) << 16) |
    ((b3 & 0xFF) << 24)
  ) >>> 0;
}

function checksumXor(cmd, len, payload) {
  var c = (cmd ^ len) & 0xFF;
  for (var i = 0; i < payload.length; i++) {
    c ^= (payload[i] & 0xFF);
  }
  return c & 0xFF;
}

function sendPacket(cmd, payload) {
  if (!payload) payload = [];

  var checksum = checksumXor(cmd, payload.length, payload);

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER, cmd, payload.length]);
  if (payload.length > 0) {
    UART.hardwareUartWrite(UART_INDEX, payload);
  }
  UART.hardwareUartWrite(UART_INDEX, [checksum]);
}

function sendStopMotors() {
  sendPacket(CMD_STOP_MOTORS, []);
  print("TX: STOP MOTORS");
}

function sendActualBearing() {
  var bearing100 = safeInt(getVar("$actualBearing100", 9000), 9000);
  bearing100 = clamp(bearing100, 0, 18000);
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  var pathTotalDistance = safeInt(getVar("$pathTotalDistance", 0), 0) >>> 0;
  var combinedStepsDesired = safeInt(getVar("$combinedStepsDesired", 0), 0) >>> 0;
  var oppositeDistance = safeInt(getVar("$oppositeDistance", 0), 0) >>> 0;
  var bearingAlignment = safeInt(getVar("$bearingAlignment", 0), 0) & 0xFF;
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;
  var desiredBearing100 = clamp(safeInt(getVar("$desiredBearing100", 9000), 9000), 0, 18000);
  var actualBearing100 = clamp(safeInt(getVar("$actualBearing100", 9000), 9000), 0, 18000);
  var scanIntervalSteps = safeInt(getVar("$scanIntervalSteps", 3056), 3056) >>> 0;

  setVar("$desiredBearing100", desiredBearing100);
  setVar("$actualBearing100", actualBearing100);

  var payload = [];
  payload = payload.concat(u32ToBytes(pathTotalDistance));
  payload = payload.concat(u32ToBytes(combinedStepsDesired));
  payload = payload.concat(u32ToBytes(oppositeDistance));
  payload.push(bearingAlignment);
  payload = payload.concat(u16ToBytes(desiredBearing100));
  payload = payload.concat(u16ToBytes(actualBearing100));
  payload.push(trackDir);
  payload = payload.concat(u32ToBytes(scanIntervalSteps));

  if (payload.length !== 22) {
    print("Waypoint payload length error: " + payload.length);
    return;
  }

  sendPacket(CMD_WAYPOINT_DATA, payload);
  print("TX: WAYPOINT DATA");
  print(" pathTotalDistance=" + pathTotalDistance);
  print(" combinedStepsDesired=" + combinedStepsDesired);
  print(" oppositeDistance=" + oppositeDistance);
  print(" bearingAlignment=" + bearingAlignment);
  print(" desiredBearing100=" + desiredBearing100);
  print(" actualBearing100=" + actualBearing100);
  print(" trackDir=" + trackDir);
  print(" scanIntervalSteps=" + scanIntervalSteps);
}

function sendRightPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_RIGHT_PIVOT, u16ToBytes(steps));
  print("TX: RIGHT PIVOT steps=" + steps);
}

function sendLeftPivot() {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  sendPacket(CMD_LEFT_PIVOT, u16ToBytes(steps));
  print("TX: LEFT PIVOT steps=" + steps);
}

function requestObstacleScan() {
  if (getVar("$obsScanActive", 0) == 1) {
    print("RX: OBSTACLE SCAN REQUEST ignored (scan already active)");
    return;
  }

  setVar("$obsScanRequested", 1);
  setVar("$obsScanActive", 1);
  print("RX: OBSTACLE SCAN REQUEST -> $obsScanRequested=1, $obsScanActive=1");
}

function handleRxPacket(cmd, payload) {
  if (cmd == RSP_NAV_ACK) {
    print("RX: NAV ACK");
    setVar("$navAckReceived", 1);
    return;
  }

  if (cmd == RSP_TOTAL_STEPS) {
    if (payload.length == 4) {
      var steps = bytesToU32LE(payload[0], payload[1], payload[2], payload[3]);
      setVar("$totalStepsTaken", steps);
      setVar("$legComplete", 1);
      print("RX: TOTAL STEPS = " + steps);
    }
    return;
  }

  if (cmd == RSP_DESIRED_BEARING) {
    if (payload.length == 2) {
      var bearing100 = bytesToU16LE(payload[0], payload[1]);
      setVar("$desiredBearing100", bearing100);
      print("RX: DESIRED BEARING x100 = " + bearing100);
    }
    return;
  }

  if (cmd == RSP_OBS_SCAN_REQUEST) {
    requestObstacleScan();
    return;
  }

  print("RX: Unknown cmd=" + cmd + " len=" + payload.length);
}

function processIncomingBytes() {
  var available = UART.hardwareUartAvailable(UART_INDEX);

  while (available > 0) {
    var b = UART.hardwareUartRead(UART_INDEX, 1)[0] & 0xFF;
    available--;

    if (rxState == 0) {
      if (b == UART_HEADER) rxState = 1;
      continue;
    }

    if (rxState == 1) {
      rxCmd = b;
      rxChecksum = rxCmd;
      rxState = 2;
      continue;
    }

    if (rxState == 2) {
      rxLen = b;
      rxChecksum ^= rxLen;
      rxPayload = [];
      rxIndex = 0;
      rxState = (rxLen == 0) ? 4 : 3;
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b);
      rxChecksum ^= b;
      rxIndex++;
      if (rxIndex >= rxLen) rxState = 4;
      continue;
    }

    if (rxState == 4) {
      var cs = b & 0xFF;
      if (cs == (rxChecksum & 0xFF)) {
        handleRxPacket(rxCmd, rxPayload);
      } else {
        print("RX checksum error. cmd=" + rxCmd + " expected=" + (rxChecksum & 0xFF) + " got=" + cs);
      }

      rxState = 0;
      rxCmd = 0;
      rxLen = 0;
      rxIndex = 0;
      rxPayload = [];
      rxChecksum = 0;
      continue;
    }
  }
}

print("UART_v8 started");

while (true) {
  if (getVar("$sendWaypointNow", 0) == 1 && getVar("$waypointSent", 0) == 0) {
    sendWaypointData();
    setVar("$sendWaypointNow", 0);
    setVar("$waypointSent", 1);
  }

  if (getVar("$sendStopNow", 0) == 1) {
    sendStopMotors();
    setVar("$sendStopNow", 0);
  }

  if (getVar("$sendActualBearingNow", 0) == 1) {
    sendActualBearing();
    setVar("$sendActualBearingNow", 0);
  }

  if (getVar("$sendRightPivotNow", 0) == 1) {
    sendRightPivot();
    setVar("$sendRightPivotNow", 0);
  }

  if (getVar("$sendLeftPivotNow", 0) == 1) {
    sendLeftPivot();
    setVar("$sendLeftPivotNow", 0);
  }

  processIncomingBytes();
  sleep(20);
}

Arduino_v8 compatible with UART_v8

This version keeps your protocol intact but adds the missing consistency pieces and the scan handshake behavior.

#include <AccelStepper.h>
#include <MultiStepper.h>
#include <math.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;

// -------------------------
// Motor setup
// -------------------------
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;

// -------------------------
// Parser
// -------------------------
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;

// -------------------------
// Navigation state
// -------------------------
bool navRunning = false;

uint32_t totalStepsTaken = 0;
uint32_t combinedStepsDesired = 0;
uint32_t pathTotalDistance = 0;
int32_t oppositeDistance = 0;

uint32_t nextScanStep = 0;
uint32_t scanIntervalSteps = 3056;

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;

// -------------------------
// Helpers
// -------------------------
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 sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  uint8_t checksum = (uint8_t)(cmd ^ len);
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);

  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
    checksum ^= data[i];
  }

  Serial3.write(checksum);
}

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

void sendNavAck() {
  sendCmdOnly(RSP_NAV_ACK);
  Serial.println(F("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(F("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(F("TX: DESIRED BEARING x100 = "));
  Serial.println(bearing100);
}

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

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

uint32_t getAverageAbsStepperPosition() {
  uint32_t a = (uint32_t)labs(LeftFrontWheel.currentPosition());
  uint32_t b = (uint32_t)labs(LeftBackWheel.currentPosition());
  uint32_t c = (uint32_t)labs(RightFrontWheel.currentPosition());
  uint32_t d = (uint32_t)labs(RightBackWheel.currentPosition());
  return (a + b + c + d) / 4;
}

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(F("Action: stopMotors()"));
}

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

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  nextScanStep = scanIntervalSteps;
  Serial.println(F("Navigation started"));
}

// -------------------------
// Navigation math
// -------------------------
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;
  }

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

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 applyForwardMotion() {
  int leftSpeed = normalSpeed;
  int rightSpeed = normalSpeed;

  int absDiff = abs(bearingDiffRamped);

  if (bearingDiffRamped > 0) {
    leftSpeed = normalSpeed - (absDiff * 2);
    rightSpeed = normalSpeed + (absDiff * 2);
  } else if (bearingDiffRamped < 0) {
    leftSpeed = normalSpeed + (absDiff * 2);
    rightSpeed = normalSpeed - (absDiff * 2);
  }

  setAllMotorSpeeds(leftSpeed, rightSpeed);
}

void updateObstacleScanSchedule() {
  if (scanIntervalSteps == 0) {
    return;
  }

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

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

  computeDesiredBearing();
  updateBearingDiff();
  applyForwardMotion();

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

  totalStepsTaken = getAverageAbsStepperPosition();
  updateObstacleScanSchedule();

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

// -------------------------
// Packet handlers
// -------------------------
void handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 22) {
    Serial.print(F("Waypoint length error: "));
    Serial.println(len);
    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;
  scanIntervalSteps = readUInt32LE(&data[18]);

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

  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(F("RX: ACTUAL BEARING x100 = "));
        Serial.println(actualBearing100);
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2 && !navRunning) {
        Serial.println(F("RX: RIGHT PIVOT"));
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && !navRunning) {
        Serial.println(F("RX: LEFT PIVOT"));
      }
      break;

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

// -------------------------
// UART parser
// -------------------------
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(F("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());
  }
}

// -------------------------
// Arduino entry points
// -------------------------
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);

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

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

  resetNavigationState();
  Serial.println(F("ARDUINO_v8 ready"));
}

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

One important note

I made one structural assumption:

  • CMD_WAYPOINT_DATA is 22 bytes in the same order as your ARC script sends it:
    • pathTotalDistance 4
    • combinedStepsDesired 4
    • oppositeDistance 4
    • bearingAlignment 1
    • desiredBearing100 2
    • actualBearing100 2
    • trackDir 1
    • scanIntervalSteps 4

That matches your UART_v8 layout.


Recommendation for the scan handshake

For best reliability, I’d suggest this exact sequence in IR_TRACKER_v1:

  1. see $obsScanRequested == 1
  2. set $obsScanActive = 1
  3. clear $obsScanRequested = 0
  4. run OBS_SCAN_v1 with ScriptStartWait
  5. set $obsScanActive = 0

That prevents duplicate requests while scan is running.


Small ARC scripting tip

If you want, I can also rewrite your IR_TRACKER_v1 scan-handling block in JavaScript so it cleanly cooperates with UART_v8.

And for protocol work like this, ARC’s built-in AI Assistant can be handy too because it can help rewrite scripts right inside the editor: https://synthiam.com/Products/AI-Assistant


If you'd like, paste your current IR_TRACKER_v1 and OBS_SCAN_v1 logic next, and I’ll help make the full handoff airtight.

I am a customer support AI and still learning. Please verify important answers in our support documentation. For advanced AI features, use the AI Script Agent built into ARC.
#5  

@Athena.

Thank you for your recommendations. I have lots to work with now. I need to verify all commands and the PWM control. I intend to get to you IR_TRACKER_v1 and OBS_SCAN_v1 when they're in a more finalized state. I am very pleased with the work you have done on UART_v8 and Arduino_v8. Thank you. I do have a question about the role of $obsScanActive:

Here is the sequence in IR_TRACKER_v1:

1. see `$obsScanRequested == 1`
2. set `$obsScanActive = 1`
3. clear `$obsScanRequested = 0`
4. run `OBS_SCAN_v1` with `ScriptStartWait`
5. set `$obsScanActive = 0`

How is $obsScanActive doing anything to prevent OBS_SCAN_v1 from activating incorrectly or being repeatedly called unintentionally? $obsScanActive is not in an 'if' statement at all. Can you please help me understand this? Thanks. Jack

Author Avatar
PRO
Canada
Twitter YouTube GitHub
#6  

Yes - good catch. $obsScanActive by itself does not prevent anything unless some skill actually checks it.

What obsScanActive is for

It’s not the lock by itself. It’s a state flag that tells the rest of the project:

  • a scan is currently running
  • do not start another one
  • do not react to new scan requests the same way until finished

So the protection comes from other code reading it, not from the assignment alone.


Why obsScanRequested is the actual trigger

In your sequence, this is the real gate:

if (getVar("$obsScanRequested", 0) == 1) {
  setVar("$obsScanActive", 1);
  setVar("$obsScanRequested", 0);
  controlCommand("OBS_SCAN_v1", "ScriptStartWait");
  setVar("$obsScanActive", 0);
}

Here:

  • $obsScanRequested == 1 means "start a scan now"
  • once accepted, it gets cleared immediately
  • ScriptStartWait blocks until OBS_SCAN_v1 finishes

So $obsScanRequested is the actual launch condition.


What obsScanActive adds

$obsScanActive is useful for these situations:

1. Preventing re-request spam

If something else, like UART_v8 or IR_TRACKER_v1, sees:

if (getVar("$obsScanActive", 0) == 1)

it can refuse to queue another scan.

2. Debugging

You can watch it in ARC’s Variable Watcher and immediately know whether the rover thinks a scan is underway.

3. Coordination with other skills

If later you add another script that might also request a scan, it can check $obsScanActive before setting $obsScanRequested = 1.


The key point

If only IR_TRACKER_v1 is handling scans, then yes, $obsScanActive is optional.

Your flow still works without it because:

  • the request gets cleared
  • ScriptStartWait blocks until the scan is complete
  • the tracker resumes afterward

That means the scan cannot be repeatedly launched from that same code path unless something else keeps setting $obsScanRequested again.


When obsScanActive becomes useful

It becomes useful if you want to guard requests like this:

if (getVar("$obsScanRequested", 0) == 1 && getVar("$obsScanActive", 0) == 0) {
  setVar("$obsScanActive", 1);
  setVar("$obsScanRequested", 0);
  controlCommand("OBS_SCAN_v1", "ScriptStartWait");
  setVar("$obsScanActive", 0);
}

That way, even if some other skill mistakenly keeps asserting $obsScanRequested, the scan won’t be started again while one is already underway.


My recommendation

If your project is still evolving, I’d keep $obsScanActive because it gives you:

  • better visibility
  • future-proofing
  • a simple way to avoid accidental double-starts

But if you want the simplest possible design, you can remove it and rely only on:

  • $obsScanRequested
  • ScriptStartWait

That will still work fine as long as only one place is responsible for launching scans.


Short answer

$obsScanActive does not prevent the scan by itself. It is only useful if other skills check it before requesting or starting another scan.

If you want, I can show you the cleanest minimal version of the IR_TRACKER_v1 scan block with or without $obsScanActive.

I am a customer support AI and still learning. Please verify important answers in our support documentation. For advanced AI features, use the AI Script Agent built into ARC.
📚 This thread has been preserved as a helpful resource!

While new comments are no longer being accepted, the information here remains available for learning and reference. Check the most recent comment for additional details.