Touch Path icon Touch Path — Sketch paths with finger or mouse and set turn/move speeds; interactive, educational demo of timing-based, sensorless robot navigation and limits. Try it →
USA
YouTube
Asked — Edited

UART_V6 Subject Line Options:- UART_V6 Waypoint Be…

Skip to comments   Jump to end

Hi @Athena,

I'm trying to make an ARC UART script that works seamlessly with an Arduino Mega. Both scripts are reproduced below. A few important constraints and clarifications:

  • For WAYPOINT DATA only: in ARC the variables $actualBearing100 and $desiredBearing100 must be the same value and equal to Servo.getPosition(d17, 0) * 100.
  • After WAYPOINT DATA is sent by the ARC UART script (UART_v6), ARC (my IR_TRACKER robot skill / script) computes $actualBearing100. The Arduino computes $desiredBearing100. These values are not read as servo positions on the Arduino - they are assigned and exchanged via ARC global variables (setVar / getVar).
  • The ARC UART_v6 robot skill picks up and sets ARC globals (getVar / setVar). The Arduino communicates over Serial3 in the sketch. The connection model appears to be a hardware UART between ARC (JavaScript running as a Robot Skill) and the Arduino Mega (Serial3).

Request:

  1. Can you rewrite a clean, full UART_v6 script (comments removed) that incorporates the difference in how $actualBearing and $desiredBearing are produced, and that matches exactly the accompanying ARDUINO_v6 sketch?

Thank you, Jack

ARDUINO_v6

#include <AccelStepper.h>
#include <MultiStepper.h>

// =====================
// Protocol
// =====================
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;

// =====================
// Motors
// =====================
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 state
// =====================
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;
uint32_t oppositeDistance = 0;
uint32_t nextScanStep = 10500;
const uint32_t scanIntervalSteps = 10500;

uint8_t bearingAlignment = 0;
uint8_t trackDir = 0;

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

int16_t bearingDiffTarget = 0;
int16_t bearingDiffRamped = 0;

uint16_t currentSpeed = 0;
uint16_t normalSpeed = 800;

// =====================
// Byte 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 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));
}

// =====================
// Protocol helpers
// =====================
uint8_t checksumXor(uint8_t cmd, uint8_t len, const uint8_t* data) {
  uint8_t c = cmd ^ len;
  for (uint8_t i = 0; i < len; i++) {
    c ^= data[i];
  }
  return c;
}

void sendPacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  Serial3.write(UART_HEADER);
  Serial3.write(cmd);
  Serial3.write(len);
  for (uint8_t i = 0; i < len; i++) {
    Serial3.write(data[i]);
  }
  Serial3.write(checksumXor(cmd, len, data));
}

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

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

void sendTotalSteps(uint32_t steps) {
  uint8_t tmp[4];
  tmp[0] = (uint8_t)(steps & 0xFF);
  tmp[1] = (uint8_t)((steps >> 8) & 0xFF);
  tmp[2] = (uint8_t)((steps >> 16) & 0xFF);
  tmp[3] = (uint8_t)((steps >> 24) & 0xFF);
  sendPacket(RSP_TOTAL_STEPS, tmp, 4);
  Serial.print("TX: TOTAL STEPS = ");
  Serial.println(steps);
}

void sendDesiredBearing(uint16_t bearing100) {
  uint8_t tmp[2];
  tmp[0] = (uint8_t)(bearing100 & 0xFF);
  tmp[1] = (uint8_t)((bearing100 >> 8) & 0xFF);
  sendPacket(RSP_DESIRED_BEARING, tmp, 2);
  lastSentDesiredBearing100 = bearing100;
  Serial.print("TX: DESIRED BEARING x100 = ");
  Serial.println(bearing100);
}

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

// =====================
// Motor helpers
// =====================
void resetStepperPositions() {
  LeftFrontWheel.setCurrentPosition(0);
  LeftBackWheel.setCurrentPosition(0);
  RightFrontWheel.setCurrentPosition(0);
  RightBackWheel.setCurrentPosition(0);
}

void setAllMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);

  LeftFrontWheel.setSpeed(leftSpeed);
  LeftBackWheel.setSpeed(leftSpeed);
  RightFrontWheel.setSpeed(rightSpeed);
  RightBackWheel.setSpeed(rightSpeed);
}

void stopMotors() {
  LeftFrontWheel.stop();
  LeftBackWheel.stop();
  RightFrontWheel.stop();
  RightBackWheel.stop();

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

  Serial.println("Action: stopMotors()");
}

void resetNavigationState() {
  resetStepperPositions();
  totalStepsTaken = 0;
  desiredBearing100 = 9000;
  actualBearing100 = 9000;
  lastSentDesiredBearing100 = 0xFFFF;
  bearingDiffTarget = 0;
  bearingDiffRamped = 0;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
  currentSpeed = 0;
  navRunning = false;

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

void beginForwardNavigation() {
  navRunning = true;
  currentSpeed = 0;
  bearingDiffRamped = 0;
  scanNeutralMode = false;
  nextScanStep = scanIntervalSteps;
}

// =====================
// 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;
    }
  }

  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 (!navRunning) {
    return;
  }

  if (currentSpeed < normalSpeed) {
    currentSpeed = normalSpeed;
  }

  computeDesiredBearing();
  updateBearingDiff();

  int leftSpeed = currentSpeed;
  int rightSpeed = currentSpeed;

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

  leftSpeed = constrain(leftSpeed, 0, 2000);
  rightSpeed = constrain(rightSpeed, 0, 2000);

  Serial.print("RUN=");
  Serial.print(navRunning);
  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);

  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 (totalStepsTaken >= nextScanStep) {
    sendObstacleScanRequest();
    scanNeutralMode = true;
    nextScanStep += scanIntervalSteps;
  }

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

// =====================
// Packet handling
// =====================
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 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 handleWaypointPacket(const uint8_t* data, uint8_t len) {
  if (len != 18) {
    Serial.println("Waypoint length error");
    return;
  }

  pathTotalDistance      = readUInt32LE(&data[0]);
  combinedStepsDesired   = readUInt32LE(&data[4]);
  oppositeDistance       = readUInt32LE(&data[8]);
  bearingAlignment       = data[12];
  desiredBearing100      = readUInt16LE(&data[13]);
  actualBearing100       = readUInt16LE(&data[15]);
  trackDir               = data[17] & 0x01;

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

  sendNavAck();
  beginForwardNavigation();
}

void handlePacket(uint8_t cmd, const uint8_t* data, uint8_t len) {
  switch (cmd) {
    case CMD_STOP_MOTORS:
      stopMotors();
      sendTotalSteps(totalStepsTaken);
      resetNavigationState();
      break;

    case CMD_ACTUAL_BEARING:
      if (len == 2) {
        actualBearing100 = readUInt16LE(data);
        Serial.print("actualBearing100: ");
        Serial.println(actualBearing100);
      }
      break;

    case CMD_RIGHT_PIVOT:
      if (len == 2 && !navRunning) {
        pivotRight(readUInt16LE(data));
      }
      break;

    case CMD_LEFT_PIVOT:
      if (len == 2 && !navRunning) {
        pivotLeft(readUInt16LE(data));
      }
      break;

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

// =====================
// 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("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 setup/loop
// =====================
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();
  runForwardLoop();
}

UART_v6 (ARC JavaScript Robot Skill)

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;

// RX parser state
var rxState = 0; // 0=wait header, 1=cmd, 2=len, 3=payload, 4=checksum
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 len = payload.length & 0xFF;
  var cs = checksumXor(cmd, len, payload);

  var out = [];
  out.push(UART_HEADER);
  out.push(cmd & 0xFF);
  out.push(len);

  for (var i = 0; i < payload.length; i++) {
    out.push(payload[i] & 0xFF);
  }

  out.push(cs);
  UART.hardwareUartWrite(UART_INDEX, out);
}

function sendCmdOnly(cmd) {
  sendPacket(cmd, []);
}

function sendStopMotors() {
  sendCmdOnly(CMD_STOP_MOTORS);
  print("TX: STOP MOTORS");
}

function sendActualBearing() {
  // You asked for bearing to reflect Servo.getPosition(d17,0) * 100
  // var bearing100 = safeInt(Servo.getPosition(d17, 0), 90) * 100;
  var bearing100 = getVar("$actualBearing100");
  bearing100 = clamp(bearing100, 0, 18000);

  var payload = u16ToBytes(bearing100);
  sendPacket(CMD_ACTUAL_BEARING, payload);

  print("TX: ACTUAL BEARING x100 = " + bearing100);
}

function sendWaypointData() {
  // Bearing source per your request
  var bearing100 = safeInt(Servo.getPosition(d17, 0), 90) * 100;
  bearing100 = clamp(bearing100, 0, 18000);

  // Build packet values from ARC globals
  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;

  // trackDir must be 0 or 1
  var trackDir = safeInt(getVar("$trackDir", 0), 0);
  trackDir = (trackDir === 0) ? 0 : 1;

  // Per your request: both values reflect Servo.getPosition(d17,0) * 100
  var desiredBearing100 = bearing100;
  var actualBearing100 = bearing100;

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

  if (payload.length != 18) {
    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);
}

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 expectedLengthForCmd(cmd) {
  switch (cmd) {
    case RSP_NAV_ACK:
      return 0;
    case RSP_TOTAL_STEPS:
      return 4;
    case RSP_DESIRED_BEARING:
      return 2;
    case RSP_OBS_SCAN_REQUEST:
      return 0;
    default:
      return -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]);
      print("RX: TOTAL STEPS = " + steps);
      setVar("$totalStepsTaken", 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];
    available--;

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

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

    if (rxState == 2) {
      rxLen = b & 0xFF;
      rxChecksum ^= rxLen;
      rxPayload = [];
      rxIndex = 0;

      if (rxLen == 0) {
        rxState = 4;
      } else {
        rxState = 3;
      }
      continue;
    }

    if (rxState == 3) {
      rxPayload.push(b & 0xFF);
      rxChecksum ^= (b & 0xFF);
      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_v5 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);
}

Related Hardware (view all EZB hardware)
EZ-B V4
EZ-B V4 by EZ-Robot
EZ-B v4/2 robot controller: dual Cortex ARM, Wi-Fi, audio/video, 24 servo/digital ports, I2C/UART, camera and Synthiam ARC control for custom robots
Wi-Fi / USB
Servos 24
Camera
Audio
UART 3
I2C
ADC 8
Digital 24

Related Robot Skills (view all robot skills)
Thumbnail
EZB Connection by Synthiam
Connects EZ-B I/O controllers to ARC via COM or IP (5 connections). Supports init scripts, battery monitoring, TCP/EZ-Script server and serial/I2C.
Requires ARC
Communication
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
Jump to end

ARC Pro

Upgrade to ARC Pro

Subscribe to ARC Pro, and your robot will become a canvas for your imagination, limited only by your creativity.

Author Avatar
PRO
Canada
Twitter YouTube GitHub
#1  

Yes - below is a cleaned-up ARC JavaScript UART_v6 script that matches your ARDUINO_v6 packet format and your clarification about WAYPOINT DATA:

  • $desiredBearing100 is produced by the Arduino and sent back to ARC
  • $actualBearing100 is produced in ARC as Servo.getPosition(d17, 0) * 100
  • For WAYPOINT DATA only, ARC sends both values in the packet, and they are intentionally the same at transmit time

I also corrected a few syntax issues from the pasted draft so it will be valid ARC JavaScript.

UART_v6 ARC JavaScript

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 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 packet = [];
  packet.push(UART_HEADER);
  packet.push(cmd & 0xFF);
  packet.push(payload.length & 0xFF);

  for (var i = 0; i < payload.length; i++) {
    packet.push(payload[i] & 0xFF);
  }

  packet.push(checksumXor(cmd, payload.length, payload));

  UART.hardwareUartWrite(UART_INDEX, packet);
}

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

function sendActualBearing() {
  var bearing100 = safeInt(Servo.getPosition(d17, 0), 0) * 100;
  sendPacket(CMD_ACTUAL_BEARING, u16ToBytes(bearing100));
  setVar("$actualBearing100", 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 bearing100 = safeInt(Servo.getPosition(d17, 0), 0) * 100;
  var desiredBearing100 = bearing100;
  var actualBearing100 = bearing100;

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

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

  if (payload.length !== 18) {
    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);
}

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 expectedLengthForCmd(cmd) {
  switch (cmd) {
    case RSP_NAV_ACK:
      return 0;
    case RSP_TOTAL_STEPS:
      return 4;
    case RSP_DESIRED_BEARING:
      return 2;
    case RSP_OBS_SCAN_REQUEST:
      return 0;
    default:
      return -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]);
      print("RX: TOTAL STEPS = " + steps);
      setVar("$totalStepsTaken", 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;

      if (rxLen == 0) {
        rxState = 4;
      } else {
        rxState = 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_v6 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);
}

Notes

A couple of important ARC points:

  • Servo.getPosition(d17, 0) is correct for ARC JavaScript
  • I kept UART_INDEX = 0 since your Arduino sketch uses Serial3, but the ARC side depends on how you configured the hardware UART in ARC
  • This script assumes the ARC robot skill is using the Hardware UART functions, not the older COM functions

One thing to verify

Your Arduino sketch uses:

  • Serial3.begin(115200);

So in ARC, make sure the UART skill / configuration matches the same baud rate and the correct hardware UART mapping.

Quick sanity check on the packet format

This script matches your Arduino protocol:

  • Header: 0xA5
  • Command
  • Length
  • Payload
  • Checksum XOR of cmd ^ len ^ payload bytes

And it uses these payload sizes:

  • CMD_STOP_MOTORS = 0
  • CMD_ACTUAL_BEARING = 2
  • CMD_RIGHT_PIVOT = 2
  • CMD_LEFT_PIVOT = 2
  • CMD_WAYPOINT_DATA = 18

If you want, I can also do one of these next:

  1. rewrite the Arduino sketch cleanly so it exactly matches this ARC script
  2. verify the packet byte layout field-by-field
  3. convert the ARC script into a reusable robot skill pattern
  4. add debug prints for every incoming/outgoing packet

If you’d like, I can also show you where to place this script in ARC and how to configure the skill from the kebab menu.


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