Translate REV 28 To C++ Teensy 4.1

Absolutely - and yes, I can clarify how the files work together.

1) doNegSearchInit() change

You want this line:

bool reflectorDetected = (irNorm > IR_THRESHOLD);

changed to:

bool reflectorDetected = (irNorm < IR_THRESHOLD);

If your sensor is active-low, that makes sense and matches the rest of your logic.

2) UART pin mapping review

Your pin plan is valid:

Teensy 4.1

• TX8 = pin 35
• RX8 = pin 34

EZ-B v4

• TX2 = D18
• RX2 = D19

That is the correct crossed-serial style wiring direction:

• Teensy TX8 (35) EZ-B RX2 (D19)
• Teensy RX8 (34) EZ-B TX2 (D18)
• GND GND

Since you mentioned the Arduino Mega uses EZ-B UART TX0/RX0, that is fine too - just keep each UART pair dedicated and matched.

3) ARC JavaScript snippet for UART START / STOP / BEARING / COUNT

Here is a simple ARC JavaScript example you can use with the EZ-B v4 UART skill or from a Script skill in ARC.

// ARC JavaScript UART example for EZ-B v4
// Assumes UART2 is used for Teensy communication on D18/D19

var uartIndex = 2;
var baud = 115200;

// Initialize UART2
UART.initHardwareUart(uartIndex, baud);

function sendCommand(cmd) {
  UART.hardwareUartWriteString(uartIndex, cmd + "\n");
  print("Sent: " + cmd);
}

function readAvailable() {
  var data = UART.hardwareUartReadStringAvailable(uartIndex);
  if (data != null && data.length > 0) {
    print("Received: " + data);
  }
}

// Example commands
sendCommand("START");
sleep(200);

sendCommand("BEARING,123");
sleep(200);

sendCommand("COUNT,18");
sleep(200);

sendCommand("STOP");
sleep(200);

// Poll for any response
readAvailable();

If you want a loop to continuously read responses

var uartIndex = 2;
var baud = 115200;

UART.initHardwareUart(uartIndex, baud);

function sendCommand(cmd) {
  UART.hardwareUartWriteString(uartIndex, cmd + "\n");
  print("Sent: " + cmd);
}

sendCommand("START");

while (true) {
  var rx = UART.hardwareUartReadStringAvailable(uartIndex);
  if (rx != null && rx.length > 0) {
    print("RX: " + rx);
  }

  sleep(100);
}

4) Do you upload each file separately?

No - you do not upload .h, .cpp, and .ino separately as separate programs.

How Arduino/Teensy IDE uses them

• .ino = the main sketch file
• .h = header file with declarations
• .cpp = implementation file with the code

They all belong to the same project folder.

Typical workflow

1. Create a folder named something like:
   • IR_TRACKERv3
2. Put these files in it:
   • IR_TRACKERv3.ino
   • IR_TRACKERv3.h
   • IR_TRACKERv3.cpp
3. Open the .ino file in Arduino IDE
4. The IDE automatically compiles the .cpp and .h files with it
5. Upload the sketch, not each file separately

5) Does .ino and .h prepare .cpp to run?

Yes - in an Arduino/Teensy project:

• .ino is the entry point
• .h tells the compiler what functions/variables exist
• .cpp contains the actual logic

So:

• .h = declarations
• .cpp = code
• .ino = setup/loop and project start

They work together as one compiled firmware image.

6) Important note about your current UART plan

If you want Teensy UART8, make sure your code is using the matching serial port object:

• On Teensy, Serial1, Serial2, etc. map to specific pins.
• TX8/RX8 is not a standard Arduino-style name unless you are specifically using a Teensy board definition or a library wrapper that exposes it that way.

On Teensy 4.1, you’ll likely need to use:

• Serial8 for pin 35/34

So in the Teensy code, your UART should probably be:

Serial8.begin(115200);

and then:

Serial8.print("START\n");

If you want, I can update the full IR_TRACKERv3.cpp to use Serial8 explicitly for those pins.

7) Recommendation

I’d suggest this setup:

• Teensy 4.1
  • Serial8 on pins 35 TX / 34 RX
• EZ-B v4
  • UART2 on D18 TX / D19 RX
• Shared baud rate:
  • 115200

If you want, I can do the next step and provide:

1. the updated IR_TRACKERv3.cpp using Serial8
2. the matching ARC JavaScript for send/receive
3. a wiring table for Teensy EZ-B UART connections

If you'd like, I can rewrite the full Teensy file now with Serial8 integrated.

@Athena. Thank you for the explanations. Can you please do the next step and provide the following?

1. the updated IR_TRACKERv3.cpp using Serial8
2. the matching ARC JavaScript for send/receive
3. a wiring table for Teensy EZ-B UART connections

Can you rewrite the full Teensy file now with Serial8 integrated? Thanks. Jack

Yes - here’s the next step with Serial8 integrated for the Teensy 4.1, plus the ARC JavaScript and a wiring table.

1) Updated IR_TRACKERv3.cpp using Serial8

This version uses:

• Teensy UART: Serial8 on TX8 pin 35 and RX8 pin 34
• EZ-B v4 UART: UART2 on D18 / D19
• newline-delimited text messages
• your requested IR threshold logic in doNegSearchInit()

#include "IR_TRACKERv3.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 lastActualBearing = -1;
int lastPublishedBearing = -1;
int prevLastPublishedBearing = -1;

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 String serialLine;
static unsigned long lastStepMs = 0;
static unsigned long lastDebugMs = 0;

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(msg);
  Serial8.print('\n');
  Serial.print("TX->EZB: ");
  Serial.println(msg);
}

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

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

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

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

        Serial.print("actualBearing: ");
        Serial.println(actualBearing);

        sendBearingUpdate();
      }
    }
  } else {
    lastActualBearing = roundedBearing;
  }
}

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

  if (candidate >= 0) {
    bearingCount++;
    publishBearing(candidate);
    Serial.print("bearingCount: ");
    Serial.println(bearingCount);
  }
}

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

  if (candidate >= 0) {
    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 (state == NEG_SEARCH_INIT) {
    beginPosSweep();
    Serial.println("POS SWEEP");
  } else {
    beginNegSweep();
    Serial.println("NEG SWEEP");
  }

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

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

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

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

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

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

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

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

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

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

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

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

  if (trimmed == CMD_START) {
    startRequested = true;
    stopRequested = false;
    sendToEZB("ACK_START");
    return;
  }

  if (trimmed == CMD_STOP) {
    stopRequested = true;
    sendBearingCount();
    sendToEZB("ACK_STOP");
    return;
  }

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

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

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

    if (c == '\n' || c == '\r') {
      if (serialLine.length() > 0) {
        handleSerialCommand(serialLine);
        serialLine = "";
      }
    } else {
      serialLine += c;
    }
  }
}

void processTrackerState() {
  if (stopRequested) {
    return;
  }

  if (!startRequested) {
    return;
  }

  unsigned long now = millis();

  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 (now - lastDebugMs >= 1000) {
    lastDebugMs = now;
    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);
  beginNegSweep();

  Serial.println("IR_TRACKERv3 READY");
  Serial.println("Teensy Serial8 active on TX8=pin 35, RX8=pin 34");
  Serial.println("Commands: START, STOP, BEARING?, COUNT?");
}

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

2) IR_TRACKERv3.ino

#include "IR_TRACKERv3.h"

void setup() {
  setupTracker();
}

void loop() {
  loopTracker();
}

3) IR_TRACKERv3.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
};

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

// -------------------- COMMANDS --------------------
static const char* CMD_START = "START";
static const char* CMD_STOP = "STOP";
static const char* CMD_BEARING = "BEARING";
static const char* CMD_COUNT = "COUNT";

extern servo trackingServo;
extern TrackerState state;

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

extern int lastActualBearing;
extern int lastPublishedBearing;
extern int prevLastPublishedBearing;

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

4) ARC JavaScript for UART send/receive

This is for the EZ-B v4 side in ARC using UART2 on D18/D19.

// ARC JavaScript - UART2 with Teensy 4.1
// EZ-B UART2: TX D18, RX D19
// Teensy UART8: TX pin 35, RX pin 34

var uartIndex = 2;
var baud = 115200;

UART.initHardwareUart(uartIndex, baud);

function sendCommand(cmd) {
  UART.hardwareUartWriteString(uartIndex, cmd + "\n");
  print("TX: " + cmd);
}

function readAvailable() {
  var data = UART.hardwareUartReadStringAvailable(uartIndex);
  if (data != null && data.length > 0) {
    print("RX: " + data);
  }
}

sendCommand("START");
sleep(200);

sendCommand("BEARING,123");
sleep(200);

sendCommand("COUNT,18");
sleep(200);

sendCommand("STOP");
sleep(200);

while (true) {
  readAvailable();
  sleep(100);
}

Optional ARC control snippet

If you want to send a command once:

UART.hardwareUartWriteString(2, "START\n");

5) UART wiring table

Teensy 4.1 to EZ-B v4

Important

• TX goes to RX
• RX goes to TX
• Ground must be shared

6) How to use the sketches

Yes - these are one Teensy project, not separate uploads.

You do this:

1. Create a folder named IR_TRACKERv3
2. Put these three files inside it:
   • IR_TRACKERv3.ino
   • IR_TRACKERv3.h
   • IR_TRACKERv3.cpp
3. Open IR_TRACKERv3.ino in Arduino IDE
4. Compile and upload to the Teensy

How they work together

• .ino = startup entry points: setup() and loop()
• .h = declarations and shared constants
• .cpp = the actual code

So yes, the .ino and .h file prepare the .cpp to compile as one project.

7) One important compatibility note

Your ARC JavaScript can send:

• START
• STOP
• BEARING,123
• COUNT,18

But the current Teensy code is mainly written to receive control commands and send status updates like:

• ACK_START
• ACK_STOP
• BEARING,xxx
• COUNT,xxx
• STOP

If you want, I can do one more refinement and make the Teensy parser fully handle inbound:

• BEARING,123
• COUNT,18

as explicit commands too.

If you'd like, I can next produce a cleaner V4 of IR_TRACKERv3 that adds:

• explicit parsing of BEARING,123 and COUNT,18
• a simple checksum packet format
• and a more robust non-blocking UART state machine

If you want that, I can generate it right away.

Thank you @Athena.