Underwater ROV Thruster Controller

#include <Arduino.h>
#include <ESP32Servo.h>
#include <HUSKYLENS.h>


// Forward declarations
int throttleToPulse(int throttle);
void writeThrusters(int left, int right, int vertical);
void stopThrusters();
void printHelp();
void processCommand(String line);
void readConsole();
void forwardSonarFrames();
void reportCameraObjects();

constexpr int ESC_LEFT_PIN = 25;
constexpr int ESC_RIGHT_PIN = 26;
constexpr int ESC_VERTICAL_PIN = 27;
constexpr int SONAR_RX_PIN = 16;
constexpr int SONAR_TX_PIN = 17;
constexpr int CAMERA_RX_PIN = 32;
constexpr int CAMERA_TX_PIN = 33;
constexpr int PWM_NEUTRAL_US = 1500;
constexpr int PWM_MIN_US = 1100;
constexpr int PWM_MAX_US = 1900;
constexpr uint32_t COMMAND_TIMEOUT_MS = 500;
constexpr uint32_t STATUS_INTERVAL_MS = 250;
constexpr uint32_t CAMERA_POLL_INTERVAL_MS = 100;

Servo leftEsc;
Servo rightEsc;
Servo verticalEsc;
HardwareSerial sonarSerial(2);
HardwareSerial cameraSerial(1);
HUSKYLENS huskylens;
int leftCommand = 0;
int rightCommand = 0;
int verticalCommand = 0;
bool cameraReady = false;
uint32_t lastCommandMs = 0;
uint32_t lastStatusMs = 0;
uint32_t lastCameraPollMs = 0;
String commandLine;

int throttleToPulse(int throttle) { return map(constrain(throttle, -100, 100), -100, 100, PWM_MIN_US, PWM_MAX_US); }
void writeThrusters(int left, int right, int vertical) { leftEsc.writeMicroseconds(throttleToPulse(left)); rightEsc.writeMicroseconds(throttleToPulse(right)); verticalEsc.writeMicroseconds(throttleToPulse(vertical)); }
void stopThrusters() { leftCommand = 0; rightCommand = 0; verticalCommand = 0; writeThrusters(0, 0, 0); }

void printHelp() {
  Serial.println("Commands: T left right vertical (-100..100), STOP, STATUS, HELP");
  Serial.println("Vision: set HuskyLens to Object Recognition and teach object IDs on its screen.");
  Serial.println("Detection output: OBJECT id,x,y,width,height.");
}

void processCommand(String line) {
  line.trim(); line.toUpperCase();
  if (line == "STOP") { stopThrusters(); lastCommandMs = millis(); Serial.println("ACK STOP"); return; }
  if (line == "HELP") { printHelp(); return; }
  if (line == "STATUS") { Serial.printf("STATUS L=%d R=%d V=%d CAMERA=%s\n", leftCommand, rightCommand, verticalCommand, cameraReady ? "READY" : "OFFLINE"); return; }
  int l, r, v;
  if (sscanf(line.c_str(), "T %d %d %d", &l, &r, &v) == 3) {
    leftCommand = constrain(l, -100, 100); rightCommand = constrain(r, -100, 100); verticalCommand = constrain(v, -100, 100);
    writeThrusters(leftCommand, rightCommand, verticalCommand); lastCommandMs = millis();
    Serial.printf("ACK T %d %d %d\n", leftCommand, rightCommand, verticalCommand);
  } else Serial.println("ERR. Type HELP.");
}

void readConsole() {
  while (Serial.available()) {
    char c = static_cast<char>(Serial.read());
    if (c == '\n' || c == '\r') { if (commandLine.length()) { processCommand(commandLine); commandLine = ""; } }
    else if (commandLine.length() < 80) commandLine += c;
  }
}

void forwardSonarFrames() {
  static uint8_t frame[128]; static size_t length = 0;
  while (sonarSerial.available()) {
    uint8_t b = static_cast<uint8_t>(sonarSerial.read());
    if (length < sizeof(frame)) frame[length++] = b; else length = 0;
    if (length >= 4 && frame[0] == 0x42 && frame[1] == 0x52) {
      uint16_t payloadLength = static_cast<uint16_t>(frame[2]) | (static_cast<uint16_t>(frame[3]) << 8);
      size_t frameLength = static_cast<size_t>(payloadLength) + 8;
      if (frameLength <= sizeof(frame) && length == frameLength) {
        Serial.print("PING_RAW "); for (size_t i = 0; i < length; ++i) Serial.printf("%02X", frame[i]); Serial.println(); length = 0;
      } else if (frameLength > sizeof(frame)) length = 0;
    } else if (length >= 2) { frame[0] = frame[length - 1]; length = 1; }
  }
}

void reportCameraObjects() {
  if (!cameraReady || millis() - lastCameraPollMs < CAMERA_POLL_INTERVAL_MS) return;
  lastCameraPollMs = millis();
  if (!huskylens.request()) { Serial.println("CAMERA_ERROR request failed"); cameraReady = false; return; }
  if (!huskylens.isLearned()) { Serial.println("CAMERA_NOT_LEARNED"); return; }
  if (!huskylens.available()) { Serial.println("NO_OBJECT"); return; }
  do {
    HUSKYLENSResult result = huskylens.read();
    if (result.command == COMMAND_RETURN_BLOCK) Serial.printf("OBJECT %d,%d,%d,%d,%d\n", result.ID, result.xCenter, result.yCenter, result.width, result.height);
  } while (huskylens.available());
}

void setup() {
  Serial.begin(115200);
  sonarSerial.begin(115200, SERIAL_8N1, SONAR_RX_PIN, SONAR_TX_PIN);
  cameraSerial.begin(9600, SERIAL_8N1, CAMERA_RX_PIN, CAMERA_TX_PIN);
  leftEsc.setPeriodHertz(50); rightEsc.setPeriodHertz(50); verticalEsc.setPeriodHertz(50);
  leftEsc.attach(ESC_LEFT_PIN, 1000, 2000); rightEsc.attach(ESC_RIGHT_PIN, 1000, 2000); verticalEsc.attach(ESC_VERTICAL_PIN, 1000, 2000);
  stopThrusters(); delay(2500);
  cameraReady = huskylens.begin(cameraSerial);
  lastCommandMs = millis();
  Serial.println("ROV controller ready; all thrusters neutral.");
  Serial.printf("HuskyLens camera: %s. Set it to Object Recognition and teach target IDs.\n", cameraReady ? "READY" : "OFFLINE");
  printHelp();
}

void loop() {
  readConsole(); forwardSonarFrames(); reportCameraObjects();
  if (millis() - lastCommandMs > COMMAND_TIMEOUT_MS && (leftCommand || rightCommand || verticalCommand)) { stopThrusters(); Serial.println("FAILSAFE: command timeout; thrusters neutral."); }
  if (millis() - lastStatusMs >= STATUS_INTERVAL_MS) { lastStatusMs = millis(); Serial.printf("STATUS L=%d R=%d V=%d CAMERA=%s\n", leftCommand, rightCommand, verticalCommand, cameraReady ? "READY" : "OFFLINE"); }
}