Smart Following Suitcase

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_VL53L0X.h>

constexpr int LEFT_FWD_PIN = 25;
constexpr int LEFT_REV_PIN = 26;
constexpr int RIGHT_FWD_PIN = 13;
constexpr int RIGHT_REV_PIN = 14;
constexpr int FRONT_XSHUT_PIN = 27;
constexpr int DROP_XSHUT_PIN = 32;
constexpr int UWB_RX_PIN = 16;
constexpr int I2C_SDA_PIN = 21;
constexpr int I2C_SCL_PIN = 22;

constexpr uint16_t PWM_HZ = 20000;
constexpr uint8_t PWM_BITS = 8;
constexpr int CRUISE_PWM = 72;
constexpr int FOLLOW_STOP_CM = 130;
constexpr int FRONT_STOP_MM = 650;
constexpr int DROP_STOP_MM = 300;
constexpr uint32_t UWB_TIMEOUT_MS = 350;
constexpr uint32_t SENSOR_INTERVAL_MS = 60;

Adafruit_VL53L0X frontSensor;
Adafruit_VL53L0X dropSensor;
bool frontReady = false;
bool dropReady = false;
int frontMm = 8190;
int dropMm = 0;
float uwbDistanceCm = 0.0f;
float uwbBearingDeg = 0.0f;
uint32_t lastUwbMs = 0;
uint32_t lastSensorMs = 0;
String uwbLine;

void setMotorChannels(int leftForward, int leftReverse, int rightForward, int rightReverse) {
  ledcWrite(LEFT_FWD_PIN, constrain(leftForward, 0, 255));
  ledcWrite(LEFT_REV_PIN, constrain(leftReverse, 0, 255));
  ledcWrite(RIGHT_FWD_PIN, constrain(rightForward, 0, 255));
  ledcWrite(RIGHT_REV_PIN, constrain(rightReverse, 0, 255));
}

void stopMotors() { setMotorChannels(0, 0, 0, 0); }

bool readDistance(Adafruit_VL53L0X &sensor, int &millimeters) {
  VL53L0X_RangingMeasurementData_t measurement;
  sensor.rangingTest(&measurement, false);
  if (measurement.RangeStatus == 4) return false;
  millimeters = measurement.RangeMilliMeter;
  return true;
}

void updateSensors() {
  if (millis() - lastSensorMs < SENSOR_INTERVAL_MS) return;
  lastSensorMs = millis();
  int value;
  if (frontReady && readDistance(frontSensor, value)) frontMm = value;
  if (dropReady && readDistance(dropSensor, value)) dropMm = value;
}

void parseUwbLine(const String &line) {
  // Configure the UWB UART bridge to emit: $UWB,<distance_cm>,<bearing_deg>\n
  if (!line.startsWith("$UWB,")) return;
  int comma = line.indexOf(',', 5);
  if (comma < 0) return;
  float distance = line.substring(5, comma).toFloat();
  float bearing = line.substring(comma + 1).toFloat();
  if (distance < 20.0f || distance > 3000.0f || bearing < -180.0f || bearing > 180.0f) return;
  uwbDistanceCm = distance;
  uwbBearingDeg = bearing;
  lastUwbMs = millis();
}

void readUwb() {
  while (Serial2.available()) {
    char c = static_cast<char>(Serial2.read());
    if (c == '\n' || c == '\r') {
      if (uwbLine.length() > 0) parseUwbLine(uwbLine);
      uwbLine = "";
    } else if (uwbLine.length() < 63) {
      uwbLine += c;
    } else {
      uwbLine = "";
    }
  }
}

bool safetyClear() {
  if (!frontReady || !dropReady) return false;
  if (frontMm < FRONT_STOP_MM) return false;
  // A much larger down-facing reading means a curb, step, or hole.
  if (dropMm <= 0 || dropMm > DROP_STOP_MM) return false;
  return true;
}

void followTag() {
  if (!safetyClear() || millis() - lastUwbMs > UWB_TIMEOUT_MS || uwbDistanceCm <= FOLLOW_STOP_CM) {
    stopMotors();
    return;
  }
  // Turn toward the wearer before advancing; reverse is never commanded.
  if (uwbBearingDeg > 18.0f) {
    setMotorChannels(CRUISE_PWM, 0, 0, 0);
  } else if (uwbBearingDeg < -18.0f) {
    setMotorChannels(0, 0, CRUISE_PWM, 0);
  } else {
    int correction = constrain(static_cast<int>(uwbBearingDeg * 1.2f), -28, 28);
    setMotorChannels(CRUISE_PWM - correction, 0, CRUISE_PWM + correction, 0);
  }
}

void setupTofSensors() {
  pinMode(FRONT_XSHUT_PIN, OUTPUT);
  pinMode(DROP_XSHUT_PIN, OUTPUT);
  digitalWrite(FRONT_XSHUT_PIN, LOW);
  digitalWrite(DROP_XSHUT_PIN, LOW);
  delay(20);
  digitalWrite(FRONT_XSHUT_PIN, HIGH);
  delay(20);
  frontReady = frontSensor.begin(0x29, false, &Wire);
  if (frontReady) frontSensor.setAddress(0x30);
  digitalWrite(DROP_XSHUT_PIN, HIGH);
  delay(20);
  dropReady = dropSensor.begin(0x29, false, &Wire);
  if (dropReady) dropSensor.setAddress(0x31);
}

void setup() {
  Serial.begin(115200);
  Serial2.begin(115200, SERIAL_8N1, UWB_RX_PIN, -1);
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  ledcAttach(LEFT_FWD_PIN, PWM_HZ, PWM_BITS);
  ledcAttach(LEFT_REV_PIN, PWM_HZ, PWM_BITS);
  ledcAttach(RIGHT_FWD_PIN, PWM_HZ, PWM_BITS);
  ledcAttach(RIGHT_REV_PIN, PWM_HZ, PWM_BITS);
  stopMotors();
  setupTofSensors();
}

void loop() {
  readUwb();
  updateSensors();
  followTag();
  delay(10);
}