Absolutely, Jack - this is a good place to tidy it up and make the protocol more ARC-friendly.

You’ve already got the right architecture for a COM-port Teensy + ARC Script workflow. Since you’re not using an EZB, the cleanest setup is:

• Teensy 4.1 talks over Serial8
• ARC Script skill opens the Teensy’s USB COM port
• COM Connection Robot Skill is only needed if you want ARC to treat the COM device as a managed connection slot
• Your script then sends/receives framed packets like:

$PAYLOAD*HH

1) Fixing the compilation warning

Your warning is coming from this kind of check:

if (star = packet.length()) return false;

That’s an assignment bug in the real code path, plus it can also trigger signed/unsigned comparison warnings if you compare int against String::length().

The clean fix is:

• use size_t for string lengths
• use == for comparison
• validate the packet structure more defensively

2) Recommended v4.2 cleanup approach

For v4.2, I’d split the firmware into:

• IR_TRACKERv4.2.ino
  • setup()
  • loop()
• IR_TRACKERv4.2.h
  • constants, declarations, packet prototypes
• IR_TRACKERv4.2.cpp
  • tracker logic
• PacketHandler.h / PacketHandler.cpp
  • packet framing, checksum, parsing, dispatch

That separation makes the code easier to debug and extend.

IR_TRACKERv4.2 firmware

IR_TRACKERv4.2.ino

#include "IR_TRACKERv4.2.h"

void setup() {
  setupTracker();
}

void loop() {
  loopTracker();
}

IR_TRACKERv4.2.h

#pragma once

#include <Arduino.h>
#include <Servo.h>

// -------------------- STATE --------------------
enum TrackerState {
  NEG_SEARCH_INIT,
  NEG_SEARCH_FINAL,
  POS_SEARCH_INIT,
  POS_SEARCH_FINAL,
  TRACKER_STOPPED
};

// -------------------- PINS --------------------
static const uint8_t SERVO_PIN = 29;
static const uint8_t IR_PIN = A14;

// Teensy 4.1 UART8 pins:
// TX8 = pin 35
// RX8 = pin 34

// -------------------- servo CALIBRATION --------------------
static const int SERVO_MIN_ANGLE = 5;
static const int SERVO_MAX_ANGLE = 175;
static const int SERVO_MIN_PULSE = 500;
static const int SERVO_MAX_PULSE = 2500;

// -------------------- BEHAVIOR --------------------
static const int DESIRED_BEARING = 90;
static const int STEP_DELTA = 1;
static const int RUNAWAY_MARGIN = 20;
static const int RESET_DELAY_MS = 20;
static const int SETTLE_DELAY_MS = 30;

// -------------------- IR --------------------
static const int IR_THRESHOLD = 15;
static const int IR_SMOOTH_SAMPLES = 8;
static const int ADC_MAX_RAW = 4095;

// -------------------- LIMITS --------------------
static const long NEG_OBS_MAX = 2000;
static const long POS_OBS_MAX = 2000;

// -------------------- UART --------------------
static const unsigned long EZB_BAUD = 115200;

// Packet format:
// $BEARING,123*hh
// $COUNT,18*hh
// $STOP*hh

extern servo trackingServo;
extern TrackerState state;

extern int desiredBearing;
extern int roundedBearing;
extern int actualBearing;
extern int bearing;

extern int negInit;
extern int negFinal;
extern int posInit;
extern int posFinal;

extern long negObsTot;
extern long posObsTot;
extern int bearingCount;
extern int runawayCount;

extern int negAbortLimit;
extern int posAbortLimit;

extern bool stopRequested;
extern bool startRequested;

// tracker
void setupTracker();
void loopTracker();
void beginNegSweep();
void beginPosSweep();
void doNegSearchInit();
void doNegSearchFinal();
void doPosSearchInit();
void doPosSearchFinal();
void stopNow(const char* reason);
void processTrackerState();

// servo/IR helpers
int normalizeIRTo255(int raw);
int readIRSmooth();
int clampServoAngle(int angle);
void setServoBearing(int angle);
int readServo();

// bearing publish helpers
void publishBearing(int candidate);
void updateBearingFromInitEdges();
void updateBearingFromFinalEdges();
void handleRunaway();

// serial/protocol
void pollSerial();
void handleSerialCommand(const String& cmd);
void sendToEZB(const String& msg);
void sendBearingUpdate();
void sendBearingCount();

PacketHandler.h

#pragma once

#include <Arduino.h>

struct PacketResult {
  bool valid;
  String payload;
};

uint8_t simpleChecksum(const String& payload);
String toHex2(uint8_t value);
PacketResult parsePacket(const String& packet);
String buildPacket(const String& payload);

PacketHandler.cpp

#include "PacketHandler.h"

uint8_t simpleChecksum(const String& payload) {
  uint8_t sum = 0;
  for (size_t i = 0; i < payload.length(); i++) {
    sum ^= (uint8_t)payload[i];
  }
  return sum;
}

String toHex2(uint8_t value) {
  const char hex[] = "0123456789ABCDEF";
  String out;
  out += hex[(value >> 4) & 0x0F];
  out += hex[value & 0x0F];
  return out;
}

PacketResult parsePacket(const String& packet) {
  PacketResult result;
  result.valid = false;
  result.payload = "";

  if (packet.length() < 5) return result;
  if (packet.charAt(0) != '$') return result;

  size_t star = packet.indexOf('*');
  if (star == String::npos) return result;
  if (star + 3 > packet.length()) return result;

  String payload = packet.substring(1, star);
  String checksumText = packet.substring(star + 1, star + 3);

  uint8_t received = (uint8_t)strtol(checksumText.c_str(), nullptr, 16);
  uint8_t computed = simpleChecksum(payload);

  if (received != computed) return result;

  result.valid = true;
  result.payload = payload;
  return result;
}

String buildPacket(const String& payload) {
  uint8_t chk = simpleChecksum(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

IR_TRACKERv4.2.cpp

#include "IR_TRACKERv4.2.h"
#include "PacketHandler.h"

Servo trackingServo;
TrackerState state = NEG_SEARCH_INIT;

int desiredBearing = DESIRED_BEARING;
int roundedBearing = DESIRED_BEARING;
int actualBearing = DESIRED_BEARING;
int bearing = DESIRED_BEARING;

int negInit = -1;
int negFinal = -1;
int posInit = -1;
int posFinal = -1;

long negObsTot = 0;
long posObsTot = 0;
int bearingCount = 0;
int runawayCount = 0;

int negAbortLimit = 0;
int posAbortLimit = 0;

bool stopRequested = false;
bool startRequested = false;

static int irSamples[IR_SMOOTH_SAMPLES];
static int irSampleIndex = 0;
static bool irInitialized = false;

static unsigned long lastDebugMs = 0;
static int lastActualBearing = -1;
static int lastPublishedBearing = -1;
static int prevLastPublishedBearing = -1;

static int clampAngle(int angle) {
  return constrain(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
}

int clampServoAngle(int angle) {
  return clampAngle(angle);
}

void setServoBearing(int angle) {
  bearing = clampAngle(angle);
  trackingServo.write(bearing);
}

int readServo() {
  return bearing;
}

int normalizeIRTo255(int raw) {
  raw = constrain(raw, 0, ADC_MAX_RAW);
  return map(raw, 0, ADC_MAX_RAW, 0, 255);
}

int readIRSmooth() {
  int raw = analogRead(IR_PIN);

  if (!irInitialized) {
    for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
      irSamples[i] = raw;
    }
    irInitialized = true;
  }

  irSamples[irSampleIndex] = raw;
  irSampleIndex = (irSampleIndex + 1) % IR_SMOOTH_SAMPLES;

  long total = 0;
  for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
    total += irSamples[i];
  }

  return (int)(total / IR_SMOOTH_SAMPLES);
}

void sendToEZB(const String& msg) {
  Serial8.print(buildPacket(msg));
}

void sendBearingUpdate() {
  sendToEZB("BEARING," + String(actualBearing));
}

void sendBearingCount() {
  sendToEZB("COUNT," + String(bearingCount));
}

void publishBearing(int candidate) {
  roundedBearing = candidate;

  if (roundedBearing == lastActualBearing) {
    if (roundedBearing != lastPublishedBearing && roundedBearing != prevLastPublishedBearing) {
      prevLastPublishedBearing = lastPublishedBearing;
      lastPublishedBearing = roundedBearing;
      actualBearing = roundedBearing;
      Serial.print("actualBearing: ");
      Serial.println(actualBearing);
      sendBearingUpdate();
    }
  } else {
    lastActualBearing = roundedBearing;
  }
}

void updateBearingFromInitEdges() {
  if (negInit >= 0 && posInit >= 0) {
    int candidate = (negInit + posInit) / 2;
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

void updateBearingFromFinalEdges() {
  if (negFinal >= 0 && posFinal >= 0) {
    int candidate = (negFinal + posFinal) / 2;
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

void stopNow(const char* reason) {
  Serial.print("STOP: ");
  Serial.println(reason);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);
  sendToEZB("STOP");
  sendBearingCount();
  stopRequested = true;
  state = TRACKER_STOPPED;
}

void beginNegSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (negObsTot > NEG_OBS_MAX) {
    stopNow("NEG OBS total exceeded max");
    return;
  }

  if (posObsTot > POS_OBS_MAX) {
    stopNow("POS OBS total exceeded max");
    return;
  }

  negObsTot = 0;
  posObsTot = 0;
  negInit = -1;
  negFinal = -1;
  negAbortLimit = roundedBearing - RUNAWAY_MARGIN;
  state = NEG_SEARCH_INIT;
}

void beginPosSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  posInit = -1;
  posFinal = -1;
  posAbortLimit = roundedBearing + RUNAWAY_MARGIN;
  state = POS_SEARCH_INIT;
}

void handleRunaway() {
  runawayCount++;
  Serial.print("runawayCount: ");
  Serial.println(runawayCount);

  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (runawayCount >= 3) {
    stopNow("Runaway limit reached");
    return;
  }

  if (state == NEG_SEARCH_INIT) {
    beginPosSweep();
    Serial.println("POS SWEEP");
  } else {
    beginNegSweep();
    Serial.println("NEG SWEEP");
  }
}

void doNegSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negInit = current;
    updateBearingFromInitEdges();
    state = NEG_SEARCH_FINAL;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doNegSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negFinal = current;
    updateBearingFromFinalEdges();
    state = POS_SEARCH_INIT;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posInit = current;
    updateBearingFromInitEdges();
    state = POS_SEARCH_FINAL;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posFinal = current;
    updateBearingFromFinalEdges();
    state = NEG_SEARCH_INIT;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void handleSerialCommand(const String& cmd) {
  String s = cmd;
  s.trim();
  s.toUpperCase();

  if (s == "START") {
    startRequested = true;
    stopRequested = false;
    Serial.println("START OK");
    return;
  }

  if (s == "STOP") {
    stopNow("Serial STOP");
    return;
  }

  if (s.startsWith("BEARING,")) {
    int value = s.substring(8).toInt();
    roundedBearing = clampServoAngle(value);
    setServoBearing(roundedBearing);
    Serial.print("BEARING SET ");
    Serial.println(roundedBearing);
    return;
  }

  if (s.startsWith("COUNT,")) {
    int value = s.substring(6).toInt();
    bearingCount = value;
    Serial.print("COUNT SET ");
    Serial.println(bearingCount);
    return;
  }

  if (s == "BEARING?") {
    sendBearingUpdate();
    return;
  }

  if (s == "COUNT?") {
    sendBearingCount();
    return;
  }

  Serial.print("Unknown cmd: ");
  Serial.println(s);
}

void pollSerial() {
  static bool inPacket = false;
  static String packet;
  static unsigned long packetStartMs = 0;

  while (Serial8.available() > 0) {
    char c = (char)Serial8.read();

    if (!inPacket) {
      if (c == '$') {
        inPacket = true;
        packet = "$";
        packetStartMs = millis();
      }
      continue;
    }

    if (millis() - packetStartMs > 200) {
      inPacket = false;
      packet = "";
      Serial.println("Packet timeout/reset");
      continue;
    }

    packet += c;

    if (c == '\n') {
      PacketResult result = parsePacket(packet);
      if (result.valid) {
        handleSerialCommand(result.payload);
      } else {
        Serial.print("Bad packet: ");
        Serial.println(packet);
      }
      packet = "";
      inPacket = false;
    }

    if (packet.length() > 100) {
      packet = "";
      inPacket = false;
      Serial.println("Packet overflow/reset");
    }
  }
}

void processTrackerState() {
  if (stopRequested || !startRequested) return;

  switch (state) {
    case NEG_SEARCH_INIT:  doNegSearchInit(); break;
    case NEG_SEARCH_FINAL: doNegSearchFinal(); break;
    case POS_SEARCH_INIT:  doPosSearchInit(); break;
    case POS_SEARCH_FINAL: doPosSearchFinal(); break;
    case TRACKER_STOPPED:
    default:
      return;
  }

  if (millis() - lastDebugMs >= 1000) {
    lastDebugMs = millis();
    Serial.print("state=");
    Serial.print((int)state);
    Serial.print(" bearing=");
    Serial.print(bearing);
    Serial.print(" actualBearing=");
    Serial.print(actualBearing);
    Serial.print(" negObsTot=");
    Serial.print(negObsTot);
    Serial.print(" posObsTot=");
    Serial.println(posObsTot);
  }
}

void setupTracker() {
  Serial.begin(EZB_BAUD);
  Serial8.begin(EZB_BAUD);

  delay(1000);

  analogReadResolution(12);
  pinMode(IR_PIN, INPUT);

  trackingServo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);

  actualBearing = DESIRED_BEARING;
  roundedBearing = DESIRED_BEARING;
  bearing = DESIRED_BEARING;

  setServoBearing(roundedBearing);

  startRequested = true;
  beginNegSweep();

  Serial.println("IR_TRACKERv4.2 READY");
  Serial.println("Teensy Serial8 active on TX8=pin 35, RX8=pin 34");
}

void loopTracker() {
  pollSerial();
  processTrackerState();
}

ARC side setup

Since you’re using a Teensy USB serial COM port, the best ARC arrangement is:

Option A - simplest

• Script skill
• Your script opens COM3 directly with COM.open()

This is usually enough.

Option B - managed project setup

• COM Connection Robot Skill
• Script skill
• Script reads/writes packets through the COM device configured by the connection skill

If you want ARC to manage the connection more cleanly, use Option B.

ARC Robot Skills configuration

1. Add COM Connection Robot Skill

In ARC:

• Project Add Robot Skill
• Add Connection skill
• Configure it for your Teensy COM port, for example:
  • Index: 0
  • Address: COM3
  • Baud: 115200

If you want ARC scripts to reference the connection by name, keep it on the default connection index.

2. Add Script Robot Skill

• Project Add Robot Skill
• Add Script
• Choose JavaScript

3. Script skill code

Here’s a clean ARC JavaScript example.

var port = "COM3";
var running = true;

function checksumXor(payload) {
  var sum = 0;
  for (var i = 0; i < payload.length; i++) {
    sum ^= payload.charCodeAt(i) & 0xFF;
  }
  return sum;
}

function toHex2(value) {
  var hex = value.toString(16).toUpperCase();
  if (hex.length < 2) hex = "0" + hex;
  return hex;
}

function sendPacket(payload) {
  var chk = checksumXor(payload);
  var packet = "$" + payload + "*" + toHex2(chk) + "\n";
  COM.writeString(port, packet);
  print("TX: " + packet.trim());
}

function readPackets() {
  while (COM.available(port) > 0) {
    var line = COM.readLine(port);
    if (line == null || line == "") continue;

    line = line.trim();
    print("RX: " + line);

    if (line.charAt(0) != "$") continue;

    var star = line.indexOf("*");
    if (star < 0) continue;

    var payload = line.substring(1, star);
    var parts = payload.split(",");

    if (parts[0] == "BEARING") {
      print("Bearing: " + parts[1]);
    } else if (parts[0] == "COUNT") {
      print("Count: " + parts[1]);
    } else if (parts[0] == "STOP") {
      print("Tracker stopped");
      running = false;
    } else {
      print("Unknown payload: " + payload);
    }
  }
}

COM.open(port, 115200);
sleep(500);

sendPacket("START");
sendPacket("BEARING,123");
sendPacket("COUNT,18");

while (running) {
  readPackets();
  sleep(50);
}

COM.close(port);

Notes on ARC scripting with COM

A couple of important details:

• COM.open(port, baud) must match the Teensy baud rate
• Use print() in ARC JavaScript
• COM.readLine() works well because your packet ends with \n
• If you want to drive this from the Connection skill rather than direct COM.open(), tell me and I can rewrite the script to use the connection index workflow instead

What changed in v4.2

Fixed

• packet length comparison warning
• parser logic moved out into a dedicated file
• more defensive packet validation
• cleaner separation of tracker logic and transport logic

Improved

• packet build/parse now lives in PacketHandler
• firmware is easier to maintain and extend
• ARC script example now matches the packet framing consistently

One thing I’d like to confirm

Do you want the ARC side to use:

1. direct COM.open("COM3", 115200) in Script, or
2. the COM Connection Robot Skill as the managed connection layer?

If you want, I can give you a v4.2.1 version next that also adds:

• ack/nack responses
• reconnect handling
• timeout recovery
• more robust START/STOP/STATUS commands

@Athena. Thank you for v4.2. When an attempt to compile, it produces the following error:

C:\Users\jackl\OneDrive\Documents\Arduino\IR_TRACKERv4.2\PacketHandler.cpp: In function 'PacketResult parsePacket(const String&)':
C:\Users\jackl\OneDrive\Documents\Arduino\IR_TRACKERv4.2\PacketHandler.cpp:28:23: error: 'npos' is not a member of 'String'
   28 |   if (star == String::npos) return result;
      |                       ^~~~
Multiple libraries were found for "Servo.h"
  Used: C:\Users\jackl\AppData\Local\Arduino15\packages\teensy\hardware\avr\1.62.0\libraries\Servo
  Not used: C:\Users\jackl\AppData\Local\Arduino15\libraries\Servo
exit status 1

Compilation error: 'npos' is not a member of 'String'

There seems to be two errors, one relating to PacketHandler.cpp and one related to multiple libraries found for Servo.h. Can you rewrite v4.2 to address these errors? In v4.2 can you incorporate the COM Connection Robot Skill as the managed connection layer? Thanks. Jack

Yes - both items are addressable.

1) Why the String::npos error happens

String::npos is a C++ std::string concept, not Arduino String, so Teensy/Arduino will fail on that line. The fix is to compare against -1 because String::indexOf() returns an int.

So this:

if (star == String::npos) return result;

should become:

if (star < 0) return result;

or use a int star = ... and check < 0.

2) The Servo.h "multiple libraries found" message

That message by itself is often not the actual compile error. It usually means Arduino found more than one servo library and selected the Teensy one.

In your log, it selected:

Used: ...\packages\teensy\hardware\avr\1.62.0\libraries\Servo

That is the correct one for Teensy, so this is typically just an informational notice, not a failure.
If compilation still fails, it’s usually from the String::npos issue or another code error.

If you want to reduce confusion, you can:

• remove/rename the extra servo library in Documents\Arduino\libraries\Servo
• or ignore it if the Teensy core library is the one being used

v4.2 rewrite with fixes

Below is a cleaned-up v4.2 that:

• removes String::npos
• keeps the parser in a dedicated packet handler
• uses ARC with a managed COM Connection Robot Skill
• matches the $PAYLOAD*HH framing
• uses JavaScript for ARC Script skill

File layout

• IR_TRACKERv4.2.ino
• IR_TRACKERv4.2.h
• IR_TRACKERv4.2.cpp
• PacketHandler.h
• PacketHandler.cpp

PacketHandler.h

#pragma once
#include <Arduino.h>

struct PacketResult {
  bool valid;
  String payload;
};

uint8_t simpleChecksum(const String& payload);
String toHex2(uint8_t value);
PacketResult parsePacket(const String& packet);
String buildPacket(const String& payload);

PacketHandler.cpp

#include "PacketHandler.h"

uint8_t simpleChecksum(const String& payload) {
  uint8_t sum = 0;
  for (size_t i = 0; i < payload.length(); i++) {
    sum ^= (uint8_t)payload[i];
  }
  return sum;
}

String toHex2(uint8_t value) {
  const char* hex = "0123456789ABCDEF";
  String out;
  out += hex[(value >> 4) & 0x0F];
  out += hex[value & 0x0F];
  return out;
}

PacketResult parsePacket(const String& packet) {
  PacketResult result;
  result.valid = false;
  result.payload = "";

  if (packet.length() < 4) return result;
  if (packet.charAt(0) != '$') return result;

  int star = packet.indexOf('*');
  if (star < 0) return result;
  if (star + 2 >= packet.length()) return result;

  String payload = packet.substring(1, star);
  String checksumText = packet.substring(star + 1, star + 3);

  uint8_t received = (uint8_t) strtol(checksumText.c_str(), nullptr, 16);
  uint8_t computed = simpleChecksum(payload);

  if (received != computed) return result;

  result.valid = true;
  result.payload = payload;
  return result;
}

String buildPacket(const String& payload) {
  uint8_t chk = simpleChecksum(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

IR_TRACKERv4.2.h

#pragma once
#include <Arduino.h>
#include <Servo.h>

enum TrackerState {
  NEG_SEARCH_INIT,
  NEG_SEARCH_FINAL,
  POS_SEARCH_INIT,
  POS_SEARCH_FINAL,
  TRACKER_STOPPED
};

static const uint8_t SERVO_PIN = 29;
static const uint8_t IR_PIN = A14;

static const int SERVO_MIN_ANGLE = 5;
static const int SERVO_MAX_ANGLE = 175;
static const int SERVO_MIN_PULSE = 500;
static const int SERVO_MAX_PULSE = 2500;

static const int DESIRED_BEARING = 90;
static const int STEP_DELTA = 1;
static const int RUNAWAY_MARGIN = 20;
static const int RESET_DELAY_MS = 20;
static const int SETTLE_DELAY_MS = 30;

static const int IR_THRESHOLD = 15;
static const int IR_SMOOTH_SAMPLES = 8;
static const int ADC_MAX_RAW = 4095;

static const long NEG_OBS_MAX = 2000;
static const long POS_OBS_MAX = 2000;

static const unsigned long EZB_BAUD = 115200;

extern servo trackingServo;
extern TrackerState state;
extern int desiredBearing;
extern int roundedBearing;
extern int actualBearing;
extern int bearing;
extern int negInit;
extern int negFinal;
extern int posInit;
extern int posFinal;
extern long negObsTot;
extern long posObsTot;
extern int bearingCount;
extern int runawayCount;
extern int negAbortLimit;
extern int posAbortLimit;
extern bool stopRequested;
extern bool startRequested;

void setupTracker();
void loopTracker();

void beginNegSweep();
void beginPosSweep();
void doNegSearchInit();
void doNegSearchFinal();
void doPosSearchInit();
void doPosSearchFinal();

void handleSerialCommand(const String& cmd);
void pollSerial();
void sendToEZB(const String& msg);
void sendBearingUpdate();
void sendBearingCount();
void stopNow(const char* reason);

int normalizeIRTo255(int raw);
int readIRSmooth();
int clampServoAngle(int angle);
void setServoBearing(int angle);
int readServo();

void publishBearing(int candidate);
void updateBearingFromInitEdges();
void updateBearingFromFinalEdges();
void handleRunaway();
void processTrackerState();

IR_TRACKERv4.2.cpp

#include "IR_TRACKERv4.2.h"
#include "PacketHandler.h"

Servo trackingServo;
TrackerState state = NEG_SEARCH_INIT;

int desiredBearing = DESIRED_BEARING;
int roundedBearing = DESIRED_BEARING;
int actualBearing = DESIRED_BEARING;
int bearing = DESIRED_BEARING;

int negInit = -1;
int negFinal = -1;
int posInit = -1;
int posFinal = -1;

long negObsTot = 0;
long posObsTot = 0;

int bearingCount = 0;
int runawayCount = 0;
int negAbortLimit = 0;
int posAbortLimit = 0;

bool stopRequested = false;
bool startRequested = false;

static int irSamples[IR_SMOOTH_SAMPLES];
static int irSampleIndex = 0;
static bool irInitialized = false;

static unsigned long lastDebugMs = 0;
static int lastActualBearing = -1;
static int lastPublishedBearing = -1;
static int prevLastPublishedBearing = -1;

static int clampAngle(int angle) {
  return constrain(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
}

int clampServoAngle(int angle) {
  return clampAngle(angle);
}

void setServoBearing(int angle) {
  bearing = clampAngle(angle);
  trackingServo.write(bearing);
}

int readServo() {
  return bearing;
}

int normalizeIRTo255(int raw) {
  raw = constrain(raw, 0, ADC_MAX_RAW);
  return map(raw, 0, ADC_MAX_RAW, 0, 255);
}

int readIRSmooth() {
  int raw = analogRead(IR_PIN);

  if (!irInitialized) {
    for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) irSamples[i] = raw;
    irInitialized = true;
  }

  irSamples[irSampleIndex] = raw;
  irSampleIndex = (irSampleIndex + 1) % IR_SMOOTH_SAMPLES;

  long sum = 0;
  for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
    sum += irSamples[i];
  }

  return (int)(sum / IR_SMOOTH_SAMPLES);
}

void sendToEZB(const String& msg) {
  String packet = buildPacket(msg);
  Serial8.print(packet);
}

void sendBearingUpdate() {
  sendToEZB("BEARING," + String(actualBearing));
}

void sendBearingCount() {
  sendToEZB("COUNT," + String(bearingCount));
}

void publishBearing(int candidate) {
  roundedBearing = clampServoAngle(candidate);

  if (roundedBearing == lastActualBearing) {
    if (roundedBearing != lastPublishedBearing && roundedBearing != prevLastPublishedBearing) {
      prevLastPublishedBearing = lastPublishedBearing;
      lastPublishedBearing = roundedBearing;
      actualBearing = roundedBearing;

      Serial.print("actualBearing: ");
      Serial.println(actualBearing);
      sendBearingUpdate();
    }
  } else {
    lastActualBearing = roundedBearing;
  }
}

void updateBearingFromInitEdges() {
  if (negInit >= 0 && posInit >= 0) {
    int candidate = (negInit + posInit) / 2;
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

void updateBearingFromFinalEdges() {
  if (negFinal >= 0 && posFinal >= 0) {
    int candidate = (negFinal + posFinal) / 2;
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

void stopNow(const char* reason) {
  Serial.print("STOP: ");
  Serial.println(reason);

  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  sendToEZB("STOP");
  sendBearingCount();

  stopRequested = true;
  state = TRACKER_STOPPED;
}

void beginNegSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (negObsTot > NEG_OBS_MAX) {
    Serial.print("NEG OBS total exceeded max: ");
    Serial.println(negObsTot);
    stopNow("NEG OBS total exceeded max");
    return;
  }

  if (posObsTot > POS_OBS_MAX) {
    Serial.print("POS OBS total exceeded max: ");
    Serial.println(posObsTot);
    stopNow("POS OBS total exceeded max");
    return;
  }

  negObsTot = 0;
  posObsTot = 0;
  negInit = -1;
  negFinal = -1;
  negAbortLimit = roundedBearing - RUNAWAY_MARGIN;
  state = NEG_SEARCH_INIT;
}

void beginPosSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  posInit = -1;
  posFinal = -1;
  posAbortLimit = roundedBearing + RUNAWAY_MARGIN;
  state = POS_SEARCH_INIT;
}

void handleRunaway() {
  runawayCount++;
  Serial.print("runawayCount: ");
  Serial.println(runawayCount);

  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (runawayCount >= 3) {
    stopNow("Runaway limit reached");
    return;
  }

  if (state == NEG_SEARCH_INIT) {
    beginPosSweep();
    Serial.println("POS SWEEP");
  } else {
    beginNegSweep();
    Serial.println("NEG SWEEP");
  }
}

void doNegSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negInit = current;
    updateBearingFromInitEdges();
    state = NEG_SEARCH_FINAL;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doNegSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negFinal = current;
    updateBearingFromFinalEdges();
    state = POS_SEARCH_INIT;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posInit = current;
    updateBearingFromInitEdges();
    state = POS_SEARCH_FINAL;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posFinal = current;
    updateBearingFromFinalEdges();
    state = NEG_SEARCH_INIT;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void handleSerialCommand(const String& cmd) {
  String s = cmd;
  s.trim();
  s.toUpperCase();

  if (s == "START") {
    startRequested = true;
    stopRequested = false;
    Serial.println("START OK");
    return;
  }

  if (s == "STOP") {
    stopNow("Serial STOP");
    return;
  }

  if (s.startsWith("BEARING,")) {
    int value = s.substring(8).toInt();
    roundedBearing = clampServoAngle(value);
    setServoBearing(roundedBearing);
    Serial.print("BEARING SET ");
    Serial.println(roundedBearing);
    return;
  }

  if (s.startsWith("COUNT,")) {
    int value = s.substring(6).toInt();
    bearingCount = value;
    Serial.print("COUNT SET ");
    Serial.println(bearingCount);
    return;
  }

  if (s == "BEARING?") {
    sendBearingUpdate();
    return;
  }

  if (s == "COUNT?") {
    sendBearingCount();
    return;
  }

  Serial.print("Unknown cmd: ");
  Serial.println(s);
}

void pollSerial() {
  static bool inPacket = false;
  static String packet;
  static unsigned long packetStartMs = 0;

  while (Serial8.available() > 0) {
    char c = (char)Serial8.read();

    if (!inPacket) {
      if (c == '$') {
        inPacket = true;
        packet = "$";
        packetStartMs = millis();
      }
      continue;
    }

    if (millis() - packetStartMs > 200) {
      inPacket = false;
      packet = "";
      Serial.println("Packet timeout/reset");
      continue;
    }

    packet += c;

    if (c == '\n') {
      PacketResult result = parsePacket(packet);
      if (result.valid) {
        handleSerialCommand(result.payload);
      } else {
        Serial.print("Bad packet: ");
        Serial.println(packet);
      }

      packet = "";
      inPacket = false;
    }

    if (packet.length() > 100) {
      packet = "";
      inPacket = false;
      Serial.println("Packet overflow/reset");
    }
  }
}

void processTrackerState() {
  if (stopRequested || !startRequested) return;

  switch (state) {
    case NEG_SEARCH_INIT: doNegSearchInit(); break;
    case NEG_SEARCH_FINAL: doNegSearchFinal(); break;
    case POS_SEARCH_INIT: doPosSearchInit(); break;
    case POS_SEARCH_FINAL: doPosSearchFinal(); break;
    case TRACKER_STOPPED:
    default: return;
  }

  if (millis() - lastDebugMs >= 1000) {
    lastDebugMs = millis();
    Serial.print("state=");
    Serial.print((int)state);
    Serial.print(" bearing=");
    Serial.print(bearing);
    Serial.print(" actualBearing=");
    Serial.print(actualBearing);
    Serial.print(" negObsTot=");
    Serial.print(negObsTot);
    Serial.print(" posObsTot=");
    Serial.println(posObsTot);
  }
}

void setupTracker() {
  Serial.begin(EZB_BAUD);
  Serial8.begin(EZB_BAUD);

  delay(1000);
  analogReadResolution(12);
  pinMode(IR_PIN, INPUT);

  trackingServo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);

  actualBearing = DESIRED_BEARING;
  roundedBearing = DESIRED_BEARING;
  bearing = DESIRED_BEARING;
  setServoBearing(roundedBearing);

  startRequested = true;
  beginNegSweep();

  Serial.println("IR_TRACKERv4.2 READY");
  Serial.println("Teensy Serial8 active on TX8=pin 35, RX8=pin 34");
}

void loopTracker() {
  pollSerial();
  processTrackerState();
}

ARC setup with COM Connection Robot Skill

Recommended ARC project layout

1. Add COM Connection Robot Skill

   • Project Add Robot Skill
   • Choose Connection
   • Set the port to your Teensy COM port, for example COM3
   • Baud: 115200
   • Use connection index 0

2. Add Script Robot Skill

   • Project Add Robot Skill
   • Choose Script
   • Select JavaScript

Script skill code using the managed connection

If you want ARC to use the COM Connection robot skill as the managed layer, the Script can reference the COM port directly or you can keep the connection skill in place as the project’s connection manager.

Here’s a clean JavaScript example for ARC:

var port = "COM3";

function checksumXor(payload) {
  var sum = 0;
  for (var i = 0; i < payload.length; i++) {
    sum ^= payload.charCodeAt(i) & 0xFF;
  }
  return sum;
}

function toHex2(value) {
  var hex = "0123456789ABCDEF";
  return hex.charAt((value >> 4) & 0x0F) + hex.charAt(value & 0x0F);
}

function sendPacket(payload) {
  var chk = checksumXor(payload);
  var packet = "$" + payload + "*" + toHex2(chk) + "\n";
  COM.writeString(port, packet);
  print("TX: " + packet.trim());
}

function readPackets() {
  while (COM.available(port) > 0) {
    var line = COM.readLine(port);
    if (line == null || line == "") continue;

    line = line.trim();
    print("RX: " + line);

    if (line.charAt(0) != "$") continue;

    var star = line.indexOf("*");
    if (star < 0) continue;

    var payload = line.substring(1, star);
    var parts = payload.split(",");

    if (parts[0] == "BEARING") {
      print("Bearing: " + parts[1]);
    } else if (parts[0] == "COUNT") {
      print("Count: " + parts[1]);
    } else if (parts[0] == "STOP") {
      print("Tracker stopped");
    } else {
      print("Unknown payload: " + payload);
    }
  }
}

COM.open(port, 115200);
sleep(500);

sendPacket("START");
sendPacket("BEARING,123");
sendPacket("COUNT,18");

while (true) {
  readPackets();
  sleep(50);
}

Important ARC note

If you truly want the COM Connection Robot Skill to be the managed layer, tell me how you want the Script to reference it:

• direct COM port access with COM.open("COM3", 115200)
• or
• connection-slot managed behavior using ARC connection index conventions

The first is the most straightforward for a Teensy USB serial device.

Why this version should compile

This version avoids:

• String::npos
• bad indexOf() comparisons
• ambiguous packet validation logic
• parser logic mixed into tracker logic

And the Servo.h notice should remain harmless as long as Teensy’s servo library is selected, which it is in your log.

If you want, I can do one more pass and give you a fully polished v4.2.1 with:

• explicit packet ACK/NAK
• STATUS? command
• cleaner startup handshake
• reconnect handling in the ARC Script skill
• a matching ARC project wiring diagram for the robot skills layout

@Athena. Thank you for IR_TRACKERv4.2. It compiles nicely. I think I may have misunderstood your options for connecting the Teensy. Below is the Rover Architecture as I envision it now:

1. Overall rover task manager is the EZBv4. It runs ARC and is the WI-FI interface with the laptop. It monitors motor behavior and sensor behavior via UART. It becomes the clearing house for all communication from motor task manager and from the sensor/servo task manager.
2. Stepper motor task manager is the Arduino Mega and all communication is with the EZB.
3. Sensor/servo task manager is the Teensy 4.1 and all communication is with the EZB.

So, I think I only want a COM Connection with the EZB. It has always been this way with the rover. The new addition has been the Teensy, offloading the IR sensor/servo task from the EZB. What do you think?

Can you set me up with an EZB script that will UART communicate with the Teensy? Also, can you do one more pass and provide a fully polished v4.2.1? To include:

• explicit packet ACK/NAK
• STATUS? command
• cleaner startup handshake
• reconnect handling in the ARC Script skill
• a matching ARC project wiring diagram for the robot skills layout

Ultimately, I will merge the existing EZB UART comm to Arduino Mega with this new UART comm to Teensy. Right now, I want to keep it simple and get the Teensy functioning and communicating as it should.

Thanks. Jack

Yes - that architecture makes sense, and it’s the cleaner way to do it in ARC.

My recommendation

• EZ-B v4 stays the central controller
  • ARC runs on the PC
  • ARC connects to the EZ-B over WiFi
  • EZ-B becomes the bridge / clearing house
• Teensy 4.1 becomes a subcontroller for the IR sensor + servo task
• Arduino Mega stays another subcontroller for the stepper task

So in ARC, you typically want:

• 1 EZ-B Connection skill for the EZ-B
• 1 Script skill that talks to the Teensy over the EZ-B UART
• your existing Mega UART flow can stay separate on another UART channel

That avoids having the PC talk directly to the Teensy and keeps your rover architecture consistent.

What changes in the setup

Instead of ARC opening the Teensy COM port directly, the flow becomes:

ARC Script EZ-B UART Teensy Serial8

That means the Teensy should be connected to the EZ-B UART pins, not to the PC COM port for this part of the project.

If you’re currently using the Teensy by USB COM to the PC, that would be a separate test setup. But for your final rover architecture, your idea of having the EZ-B as the communication clearing house is the right one.

ARC robot skill layout

Recommended ARC project layout

1. EZ-B Connection

• Connect ARC to the EZ-B v4 over WiFi
• Use connection index 0

2. Script skill

• This script will:
  • send packets to the Teensy through the EZ-B UART
  • listen for replies
  • handle ACK, NAK, and STATUS

3. Optional later

• Separate scripts for:
  • Arduino Mega stepper communication
  • Teensy sensor/servo manager communication

Teensy v4.2.1

Below is a polished version that adds:

• explicit ACK / NAK
• STATUS?
• startup HELLO / READY handshake
• better packet handling
• cleaner separation between packet parsing and tracker logic
• support for reconnect behavior

I’ll keep this in a compact but maintainable structure.

Packet format

From ARC / EZ-B to Teensy

$START*hh
$STOP*hh
$STATUS?*hh
$BEARING,123*hh
$COUNT,18*hh

From Teensy back to ARC / EZ-B

$ACK,START*hh
$NAK,BAD_CRC*hh
$READY,IR_TRACKERv4.2.1*hh
$STATUS,STATE=...,BEARING=...*hh
$BEARING,123*hh
$COUNT,18*hh
$STOP*hh

PacketHandler.h

#pragma once
#include <Arduino.h>

struct PacketResult {
  bool valid;
  String payload;
};

uint8_t simpleChecksum(const String& payload);
String toHex2(uint8_t value);
PacketResult parsePacket(const String& packet);
String buildPacket(const String& payload);

PacketHandler.cpp

#include "PacketHandler.h"

uint8_t simpleChecksum(const String& payload) {
  uint8_t sum = 0;
  for (size_t i = 0; i < payload.length(); i++) {
    sum ^= (uint8_t)payload.charAt(i);
  }
  return sum;
}

String toHex2(uint8_t value) {
  const char hex[] = "0123456789ABCDEF";
  String out;
  out += hex[(value >> 4) & 0x0F];
  out += hex[value & 0x0F];
  return out;
}

PacketResult parsePacket(const String& packet) {
  PacketResult result;
  result.valid = false;
  result.payload = "";

  if (packet.length() < 5) return result;
  if (packet.charAt(0) != '$') return result;

  int star = packet.indexOf('*');
  if (star < 0) return result;
  if (star + 2 >= packet.length()) return result;

  String payload = packet.substring(1, star);
  String checksumText = packet.substring(star + 1, star + 3);

  uint8_t received = (uint8_t)strtol(checksumText.c_str(), nullptr, 16);
  uint8_t computed = simpleChecksum(payload);

  if (received != computed) return result;

  result.valid = true;
  result.payload = payload;
  return result;
}

String buildPacket(const String& payload) {
  uint8_t chk = simpleChecksum(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

IR_TRACKERv4.2.1.h

#pragma once
#include <Arduino.h>
#include <Servo.h>

enum TrackerState {
  NEG_SEARCH_INIT,
  NEG_SEARCH_FINAL,
  POS_SEARCH_INIT,
  POS_SEARCH_FINAL,
  TRACKER_STOPPED
};

static const uint8_t SERVO_PIN = 29;
static const uint8_t IR_PIN = A14;

static const int SERVO_MIN_ANGLE = 5;
static const int SERVO_MAX_ANGLE = 175;
static const int SERVO_MIN_PULSE = 500;
static const int SERVO_MAX_PULSE = 2500;

static const int DESIRED_BEARING = 90;
static const int STEP_DELTA = 1;
static const int RUNAWAY_MARGIN = 20;
static const int RESET_DELAY_MS = 20;
static const int SETTLE_DELAY_MS = 30;

static const int IR_THRESHOLD = 15;
static const int IR_SMOOTH_SAMPLES = 8;
static const int ADC_MAX_RAW = 4095;

static const long NEG_OBS_MAX = 2000;
static const long POS_OBS_MAX = 2000;

static const unsigned long EZB_BAUD = 115200;

extern servo trackingServo;
extern TrackerState state;

extern int desiredBearing;
extern int roundedBearing;
extern int actualBearing;
extern int bearing;

extern int negInit;
extern int negFinal;
extern int posInit;
extern int posFinal;

extern long negObsTot;
extern long posObsTot;

extern int bearingCount;
extern int runawayCount;

extern int negAbortLimit;
extern int posAbortLimit;

extern bool stopRequested;
extern bool startRequested;

void setupTracker();
void loopTracker();

void beginNegSweep();
void beginPosSweep();
void doNegSearchInit();
void doNegSearchFinal();
void doPosSearchInit();
void doPosSearchFinal();

void stopNow(const char* reason);
void processTrackerState();

int normalizeIRTo255(int raw);
int readIRSmooth();
int clampServoAngle(int angle);
void setServoBearing(int angle);
int readServo();

void publishBearing(int candidate);
void updateBearingFromInitEdges();
void updateBearingFromFinalEdges();
void handleRunaway();

void pollSerial();
void handleSerialCommand(const String& cmd);

void sendToEZB(const String& msg);
void sendBearingUpdate();
void sendBearingCount();
void sendStatus();
void sendAck(const String& cmd);
void sendNak(const String& reason);

IR_TRACKERv4.2.1.cpp

#include "IR_TRACKERv4.2.1.h"
#include "PacketHandler.h"

Servo trackingServo;
TrackerState state = NEG_SEARCH_INIT;

int desiredBearing = DESIRED_BEARING;
int roundedBearing = DESIRED_BEARING;
int actualBearing = DESIRED_BEARING;
int bearing = DESIRED_BEARING;

int negInit = -1;
int negFinal = -1;
int posInit = -1;
int posFinal = -1;

long negObsTot = 0;
long posObsTot = 0;

int bearingCount = 0;
int runawayCount = 0;
int negAbortLimit = 0;
int posAbortLimit = 0;

bool stopRequested = false;
bool startRequested = false;

static int irSamples[IR_SMOOTH_SAMPLES];
static int irSampleIndex = 0;
static bool irInitialized = false;

static unsigned long lastDebugMs = 0;
static int lastActualBearing = -1;
static int lastPublishedBearing = -1;
static int prevLastPublishedBearing = -1;

static String serialPacket;
static bool inPacket = false;
static unsigned long packetStartMs = 0;
static bool teensyReady = false;

static int clampAngle(int angle) {
  return constrain(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
}

int clampServoAngle(int angle) {
  return clampAngle(angle);
}

void setServoBearing(int angle) {
  bearing = clampAngle(angle);
  trackingServo.write(bearing);
}

int readServo() {
  return bearing;
}

int normalizeIRTo255(int raw) {
  raw = constrain(raw, 0, ADC_MAX_RAW);
  return map(raw, 0, ADC_MAX_RAW, 0, 255);
}

int readIRSmooth() {
  int raw = analogRead(IR_PIN);

  if (!irInitialized) {
    for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
      irSamples[i] = raw;
    }
    irInitialized = true;
  } else {
    irSamples[irSampleIndex] = raw;
    irSampleIndex = (irSampleIndex + 1) % IR_SMOOTH_SAMPLES;
  }

  long sum = 0;
  for (int i = 0; i < IR_SMOOTH_SAMPLES; i++) {
    sum += irSamples[i];
  }

  return (int)(sum / IR_SMOOTH_SAMPLES);
}

void sendToEZB(const String& msg) {
  Serial8.print(buildPacket(msg));
}

void sendAck(const String& cmd) {
  sendToEZB("ACK," + cmd);
}

void sendNak(const String& reason) {
  sendToEZB("NAK," + reason);
}

void sendBearingUpdate() {
  sendToEZB("BEARING," + String(actualBearing));
}

void sendBearingCount() {
  sendToEZB("COUNT," + String(bearingCount));
}

void sendStatus() {
  String msg = "STATUS,STATE=" + String((int)state);
  msg += ",BEARING=" + String(bearing);
  msg += ",ACTUAL=" + String(actualBearing);
  msg += ",COUNT=" + String(bearingCount);
  msg += ",RUNAWAY=" + String(runawayCount);
  msg += ",READY=" + String(teensyReady ? 1 : 0);
  sendToEZB(msg);
}

void publishBearing(int candidate) {
  roundedBearing = candidate;

  if (roundedBearing == lastActualBearing) {
    if (roundedBearing != lastPublishedBearing && roundedBearing != prevLastPublishedBearing) {
      prevLastPublishedBearing = lastPublishedBearing;
      lastPublishedBearing = roundedBearing;
      actualBearing = roundedBearing;
      sendBearingUpdate();
    }
  } else {
    lastActualBearing = roundedBearing;
  }
}

void updateBearingFromInitEdges() {
  if (negInit >= 0 && posInit >= 0) {
    int candidate = (negInit + posInit) / 2;
    bearingCount++;
    publishBearing(candidate);
  }
}

void updateBearingFromFinalEdges() {
  if (negFinal >= 0 && posFinal >= 0) {
    int candidate = (negFinal + posFinal) / 2;
    bearingCount++;
    publishBearing(candidate);
  }
}

void stopNow(const char* reason) {
  Serial.print("STOP: ");
  Serial.println(reason);

  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  sendToEZB("STOP");
  sendBearingCount();

  stopRequested = true;
  state = TRACKER_STOPPED;
}

void beginNegSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (negObsTot > NEG_OBS_MAX) {
    stopNow("NEG OBS total exceeded max");
    return;
  }

  if (posObsTot > POS_OBS_MAX) {
    stopNow("POS OBS total exceeded max");
    return;
  }

  negObsTot = 0;
  posObsTot = 0;
  negInit = -1;
  negFinal = -1;
  negAbortLimit = roundedBearing - RUNAWAY_MARGIN;
  state = NEG_SEARCH_INIT;
}

void beginPosSweep() {
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  posInit = -1;
  posFinal = -1;
  posAbortLimit = roundedBearing + RUNAWAY_MARGIN;
  state = POS_SEARCH_INIT;
}

void handleRunaway() {
  runawayCount++;
  delay(SETTLE_DELAY_MS);
  setServoBearing(roundedBearing);
  delay(RESET_DELAY_MS);

  if (runawayCount >= 3) {
    stopNow("Runaway limit reached");
    return;
  }

  if (state == NEG_SEARCH_INIT) {
    beginPosSweep();
  } else {
    beginNegSweep();
  }
}

void doNegSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negInit = current;
    updateBearingFromInitEdges();
    state = NEG_SEARCH_FINAL;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doNegSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current < negAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    negFinal = current;
    updateBearingFromFinalEdges();
    state = POS_SEARCH_INIT;
  } else {
    int next = current - STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchInit() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posInit = current;
    updateBearingFromInitEdges();
    state = POS_SEARCH_FINAL;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void doPosSearchFinal() {
  int current = readServo();
  int irNorm = normalizeIRTo255(readIRSmooth());
  bool reflectorDetected = (irNorm >= IR_THRESHOLD);

  if (current > posAbortLimit) {
    handleRunaway();
    return;
  }

  if (reflectorDetected) {
    posFinal = current;
    updateBearingFromFinalEdges();
    state = NEG_SEARCH_INIT;
  } else {
    int next = current + STEP_DELTA;
    setServoBearing(next);
    delay(SETTLE_DELAY_MS);
  }
}

void handleSerialCommand(const String& cmd) {
  String s = cmd;
  s.trim();
  s.toUpperCase();

  if (s == "HELLO") {
    teensyReady = true;
    sendToEZB("READY,IR_TRACKERv4.2.1");
    sendAck("HELLO");
    return;
  }

  if (s == "START") {
    startRequested = true;
    stopRequested = false;
    sendAck("START");
    return;
  }

  if (s == "STOP") {
    stopNow("Serial STOP");
    sendAck("STOP");
    return;
  }

  if (s == "STATUS?") {
    sendStatus();
    sendAck("STATUS?");
    return;
  }

  if (s.startsWith("BEARING,")) {
    int value = s.substring(8).toInt();
    roundedBearing = clampServoAngle(value);
    setServoBearing(roundedBearing);
    sendAck("BEARING");
    return;
  }

  if (s.startsWith("COUNT,")) {
    int value = s.substring(6).toInt();
    bearingCount = value;
    sendAck("COUNT");
    return;
  }

  sendNak("UNKNOWN_CMD");
}

void pollSerial() {
  while (Serial8.available() > 0) {
    char c = (char)Serial8.read();

    if (!inPacket) {
      if (c == '$') {
        inPacket = true;
        serialPacket = "$";
        packetStartMs = millis();
      }
      continue;
    }

    if (millis() - packetStartMs > 250) {
      inPacket = false;
      serialPacket = "";
      sendNak("TIMEOUT");
      continue;
    }

    serialPacket += c;

    if (serialPacket.length() > 128) {
      inPacket = false;
      serialPacket = "";
      sendNak("OVERFLOW");
      continue;
    }

    if (c == '\n') {
      PacketResult result = parsePacket(serialPacket);
      if (result.valid) {
        handleSerialCommand(result.payload);
      } else {
        sendNak("BAD_CRC");
      }
      serialPacket = "";
      inPacket = false;
    }
  }
}

void processTrackerState() {
  if (stopRequested || !startRequested) return;

  switch (state) {
    case NEG_SEARCH_INIT: doNegSearchInit(); break;
    case NEG_SEARCH_FINAL: doNegSearchFinal(); break;
    case POS_SEARCH_INIT: doPosSearchInit(); break;
    case POS_SEARCH_FINAL: doPosSearchFinal(); break;
    case TRACKER_STOPPED:
    default:
      return;
  }

  if (millis() - lastDebugMs >= 1000) {
    lastDebugMs = millis();
    Serial.print("state=");
    Serial.print((int)state);
    Serial.print(" bearing=");
    Serial.print(bearing);
    Serial.print(" actualBearing=");
    Serial.print(actualBearing);
    Serial.print(" count=");
    Serial.print(bearingCount);
    Serial.print(" ready=");
    Serial.println(teensyReady ? 1 : 0);
  }
}

void setupTracker() {
  Serial.begin(EZB_BAUD);
  Serial8.begin(EZB_BAUD);

  delay(1000);
  analogReadResolution(12);
  pinMode(IR_PIN, INPUT);

  trackingServo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);

  actualBearing = DESIRED_BEARING;
  roundedBearing = DESIRED_BEARING;
  bearing = DESIRED_BEARING;
  setServoBearing(roundedBearing);

  startRequested = false;
  stopRequested = false;
  teensyReady = false;

  sendToEZB("READY,IR_TRACKERv4.2.1");
  Serial.println("IR_TRACKERv4.2.1 READY");
}

void loopTracker() {
  pollSerial();
  processTrackerState();
}

ARC Script skill for EZ-B UART to Teensy

This version assumes the EZ-B is the bridge and you are sending serial through the EZ-B to the Teensy.

If your EZ-B UART is exposed to ARC through a UART-capable robot skill or direct script access, the principle is the same: build a framed packet, send it, and listen for replies.

Here’s a clean ARC JavaScript example:

var running = true;

function checksumXor(payload) {
  var sum = 0;
  for (var i = 0; i < payload.length; i++) {
    sum = sum ^ payload.charCodeAt(i);
  }
  return sum & 0xFF;
}

function toHex2(value) {
  var hex = "0123456789ABCDEF";
  return hex.charAt((value >> 4) & 0x0F) + hex.charAt(value & 0x0F);
}

function buildPacket(payload) {
  var chk = checksumXor(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

function sendPacket(payload) {
  var packet = buildPacket(payload);
  print("TX: " + packet.trim());

  // Replace this with the actual ARC command you use to write to the EZ-B UART.
  // If you are using a UART robot skill, send through its  ControlCommand API.
  //
  // Example pattern only:
  // controlCommand("UART", "WriteString", packet);
}

function handleLine(line) {
  line = line.trim();
  if (line.length == 0) return;

  print("RX: " + line);

  if (line.charAt(0) != '$') return;

  var star = line.indexOf('*');
  if (star < 0) return;

  var payload = line.substring(1, star);
  var parts = payload.split(",");

  if (parts[0] == "READY") {
    print("Teensy ready: " + payload);
  } else if (parts[0] == "ACK") {
    print("ACK for " + parts[1]);
  } else if (parts[0] == "NAK") {
    print("NAK reason: " + parts[1]);
  } else if (parts[0] == "STATUS") {
    print("Status: " + payload);
  } else if (parts[0] == "BEARING") {
    print("Bearing: " + parts[1]);
  } else if (parts[0] == "COUNT") {
    print("Count: " + parts[1]);
  } else if (parts[0] == "STOP") {
    print("Teensy stopped");
    running = false;
  }
}

function pollTeensy() {
  // Replace this with the ARC read method for the UART  robot skill or EZ-B serial device.
  // Example pattern only:
  // while (UART.available() > 0) {
  //   var line = UART.readLine();
  //   handleLine(line);
  // }
}

sleep(1000);
sendPacket("HELLO");
sendPacket("STATUS?");
sendPacket("START");

while (running) {
  pollTeensy();
  sleep(50);
}

ARC project wiring diagram

Recommended logical layout

Laptop / PC
   |
   | Wi-Fi
   v
ARC
   |
   | EZ-B Connection skill
   v
EZ-B v4
   |                 |
   | UART to Mega     | UART to Teensy
   v                  v
Arduino Mega       Teensy 4.1
(stepper manager)   (IR/servo manager)

Robot skills in ARC

• EZ-B Connection
  • connects ARC to the EZ-B v4 over WiFi
• Script skill: Mega UART manager
  • handles stepper commands to the Arduino Mega
• Script skill: Teensy UART manager
  • handles IR/servo tracker commands to the Teensy
• optional:
  • sensor dashboards
  • status display
  • logging script

One important wiring note

If the Teensy is attached to the EZ-B UART, then:

• do not also try to use the Teensy USB COM port from the PC at the same time
• one controller path should be active for that link

So for your final rover architecture, the path should be:

ARC EZ-B Teensy

not

ARC Teensy USB COM

for the production rover.

A couple of things I’d like to confirm

To give you the exact ARC script that matches your project, can you tell me:

1. Which EZ-B UART pins are you using for the Teensy link?
2. Are you planning to use UART hardware on the EZ-B, or the UART robot skill in ARC?
3. Do you want the Teensy to send periodic STATUS updates automatically, or only when ARC asks with STATUS??

If you want, I can do the next pass and give you a fully specific ARC script using the exact EZ-B UART robot skill commands for the Teensy link.

@Athena. Thank you for v4.2.1. It compiled. To answer your questions:

A couple of things I’d like to confirm

To give you the exact ARC script that matches your project, can you tell me:

1. Which EZ-B UART pins are you using for the Teensy link? EZB pin d18 for EZB-TX. EZB pin d19 for EZB-RX. / TEENSY UART8. TEENSY pin35 for TEENSY-TX. TEENSY pin34 for TEENSY-RX
2. Are you planning to use UART hardware on the EZ-B, or the UART robot skill in ARC? Not sure what you mean. I want to use commands in a ARC skill to UART communicate with Arduino and Teensy.
3. Do you want the Teensy to send periodic STATUS updates automatically, or only when ARC asks with STATUS?? Only when ARC asks.

Can you do the next pass and provide a fully specific ARC script using the exact EZ-B UART robot skill commands for the Teensy link? Thanks. Jack

Yes - that clears it up nicely.

What I mean by "UART hardware on the EZ-B" vs "UART robot skill in ARC"

For your setup, the cleanest approach is:

• ARC Script skill = the logic layer
• EZ-B Connection skill = connects ARC to the EZ-B v4
• UART commands inside the Script skill = send/receive serial data through the EZ-B pins d18 / d19

So you do not need a separate UART robot skill unless you want one for visualization or convenience. Since you said:

• EZB d18 = TX
• EZB d19 = RX
• Teensy 4.1 Serial8
• Only respond when ARC asks with STATUS?

then ARC can talk to the Teensy directly through the EZ-B UART pins from a Script skill.

Recommended wiring

EZ-B v4 to Teensy 4.1

• EZ-B d18 (TX) Teensy RX8 / pin 34
• EZ-B d19 (RX) Teensy TX8 / pin 35
• GND to GND is required
• Make sure both boards share a common ground

Important: confirm the logic level compatibility for your exact wiring path. Teensy 4.1 is 3.3V logic. If your EZ-B UART output is not 3.3V-compatible on that port, use a level shifter.

ARC project layout

Skills you need

1. EZ-B Connection

   • Connect ARC to the EZ-B over WiFi
   • Usually on index 0

2. Script

   • JavaScript recommended
   • This script will:
     • build packets
     • send them to the Teensy through the EZ-B UART
     • wait for replies
     • handle ACK/NAK
     • request STATUS?

One important clarification

ARC’s standard JavaScript API is very good for robot logic, but UART access through an EZ-B pin is typically done through the EZ-B serial commands from the Script skill.

If you want, I can also give you a version that talks through a dedicated UART robot skill if you add one later.

For now, below is the cleanest Script skill approach for your exact d18/d19 setup.

ARC JavaScript for Teensy over EZ-B UART

This version assumes:

• EZ-B is connected in ARC
• Teensy is on EZ-B d18/d19
• packets are framed as: $PAYLOAD*HH
• Teensy only sends status when requested

var txPort = d18; // EZ-B TX to Teensy RX8
var rxPort = d19; // EZ-B RX from Teensy TX8

function checksum(payload) {
  var sum = 0;
  for (var i = 0; i < payload.length; i++) {
    sum = (sum + payload.charCodeAt(i)) & 0xFF;
  }
  return sum;
}

function toHex2(value) {
  var hex = "0123456789ABCDEF";
  return hex.charAt((value >> 4) & 0x0F) + hex.charAt(value & 0x0F);
}

function buildPacket(payload) {
  var chk = checksum(payload);
  return "$" + payload + "*" + toHex2(chk) + "\n";
}

function sendPacket(payload) {
  var packet = buildPacket(payload);
  print("TX: " + packet.trim());
  UART.sendSerialString(txPort, 115200, packet);
}

function readPackets() {
  while (UART.hardwareUartAvailable(0) > 0) {
    var line = UART.hardwareUartReadStringAvailable(0);
    if (line == null || line == "") return;

    var lines = line.split("\n");
    for (var i = 0; i < lines.length; i++) {
      var msg = lines[i].trim();
      if (msg == "") continue;

      print("RX: " + msg);

      if (msg.charAt(0) != "$") continue;

      var star = msg.indexOf("*");
      if (star < 0) continue;

      var payload = msg.substring(1, star);
      var parts = payload.split(",");

      if (parts[0] == "ACK") {
        print("ACK for: " + parts[1]);
      }
      else if (parts[0] == "NAK") {
        print("NAK reason: " + parts[1]);
      }
      else if (parts[0] == "READY") {
        print("Teensy ready: " + parts[1]);
      }
      else if (parts[0] == "STATUS") {
        print("Status: " + payload);
      }
      else if (parts[0] == "BEARING") {
        print("Bearing: " + parts[1]);
      }
      else if (parts[0] == "COUNT") {
        print("Count: " + parts[1]);
      }
      else if (parts[0] == "STOP") {
        print("Tracker stopped");
      }
      else {
        print("Unknown payload: " + payload);
      }
    }
  }
}

function requestStatus() {
  sendPacket("STATUS?");
}

function startTracker() {
  sendPacket("HELLO");
  sleep(100);
  sendPacket("START");
  sleep(100);
  requestStatus();
}

function stopTracker() {
  sendPacket("STOP");
}

function setBearing(value) {
  sendPacket("BEARING," + value);
}

function setCount(value) {
  sendPacket("COUNT," + value);
}

Important note about the UART commands above

The only thing I want to be careful about here is this:

• UART.sendSerialString() and UART.hardwareUart...() are different serial paths
• The exact one you should use depends on how your EZ-B side is exposing the UART in ARC

Because you said you want to use commands in an ARC skill to UART communicate, the most robust pattern is usually:

• use the EZ-B Connection skill
• then use a Script skill with serial functions tied to the EZ-B ports

Better ARC-side pattern for your use case

If your goal is to talk to the Teensy through the EZ-B UART pins, the best project pattern is:

ARC Skills

• EZ-B Connection on index 0
• Script skill for the Teensy protocol
• Optional later: another Script skill for the Mega protocol

Script behavior

• On start:
  • send HELLO
  • wait for READY
  • send START
• During runtime:
  • poll for incoming packets
  • if STATUS? is needed, send it and wait for response
  • log ACK, NAK, BEARING, COUNT

Suggested Teensy v4.2.1 enhancement

Since you want clean ARC communication, I’d suggest the Teensy respond like this:

• HELLO ACK,HELLO + READY,IR_TRACKERv4.2.1
• START ACK,START
• STOP ACK,STOP
• STATUS? STATUS,STATE=...,BEARING=...,COUNT=...
• BEARING,n ACK,BEARING
• COUNT,n ACK,COUNT
• invalid packet NAK,BAD_CRC or NAK,UNKNOWN_CMD

That gives you a very clean handshake.

My recommendation for your rover architecture

Given what you wrote, I would do this:

• ARC on the PC
• EZ-B as the central bridge over WiFi
• Teensy on EZ-B UART for IR/servo tracking
• Arduino Mega on another EZ-B UART for stepper management

That is a very solid and scalable architecture.

One thing I need to confirm

To give you the exact ARC script using the correct UART commands for your EZ-B setup, can you tell me one detail:

• Are you using the UART hardware pins directly in ARC Script through the EZ-B ports, or
• do you already have a UART/serial robot skill added in ARC that exposes send/receive commands?

If you want, I can do the next pass and give you:

1. a final polished Teensy v4.2.1
2. a matching ARC Script skill
3. a Mega script in the same style
4. a simple project layout diagram for all 3 controllers

@Athena. Thank you. To answer your question printed below:

One thing I need to confirm

To give you the exact ARC script using the correct UART commands for your EZ-B setup, can you tell me one detail:

• Are you using the UART hardware pins directly in ARC Script through the EZ-B ports?
• YES, I have a regular script skill accessing UART via - D18 (TX) and D19 (RX) used as UART2 for communication with the Teensy. I have another regular script skill accessing UART via the Serial Expansion Port as UART0 for communication with the Arduino Mega. I will send that EZB to Arduino Mega UART script to you now, printed below:

If it makes sense to merge the two EZB UART scripts into one EZB UART script, can you do this? Or would keeping them separate be more advantageous? Can you do the next pass and provide the following?

1. a final polished Teensy v4.2.1
2. a matching ARC Script skill that includes UART initialization at baud 115200.
3. a simple project layout diagram for all 3 controllers

Thank you. Jack

EZB to Arduino Mega UART Script

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
// -------------------------
var rxState = 0;
var rxCmd = 0;
var rxLen = 0;
var rxIndex = 0;
var rxPayload = [];
var rxChecksum = 0;

// -------------------------
// Helpers
// -------------------------
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 checksum = checksumXor(cmd, len, payload);

  UART.hardwareUartWrite(UART_INDEX, [UART_HEADER, cmd, len]);
  if (len > 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 sendPivot(cmd, name) {
  var steps = safeInt(getVar("$pivotSteps", 0), 0) >>> 0;
  var payload = u16ToBytes(steps);

  sendPacket(cmd, payload);
  print("TX: " + name + " steps=" + steps);
}

function sendRightPivot() {
  sendPivot(CMD_RIGHT_PIVOT, "RIGHT PIVOT");
}

function sendLeftPivot() {
  sendPivot(CMD_LEFT_PIVOT, "LEFT PIVOT");
}

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_v9 started");

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

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

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