Autonomous 6x6 AI Rover

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MCP23X17.h>
#include <DHT.h>


// Hoisted type definitions
enum RoverMode { STOPPED, MANUAL, LINE_FOLLOW, AVOID };


// Forward declarations
void setDirections(bool leftForward, bool rightForward);
void setMotorPwm(uint8_t leftSpeed, uint8_t rightSpeed);
void drive(int leftSpeed, int rightSpeed);
void emergencyStop();
long readDistanceCm();
void lineFollowStep();
void obstacleAvoidStep();
void sendTelemetry();
void handleCommand(String command);
void pollCommands(Stream &port, String &buffer);

constexpr uint8_t LEFT_A_PWM = 25, LEFT_B_PWM = 26, LEFT_STBY = 27;
constexpr uint8_t RIGHT_A_PWM = 32, RIGHT_B_PWM = 33, RIGHT_STBY = 5;
constexpr uint8_t ULTRASONIC_TRIG = 23, ULTRASONIC_ECHO = 15;
constexpr uint8_t DHT_PIN = 4, GAS_ADC = 35;
constexpr uint8_t LINE_LEFT_OUTER = 13, LINE_LEFT_INNER = 14;
constexpr uint8_t LINE_RIGHT_INNER = 18, LINE_RIGHT_OUTER = 19;
constexpr uint8_t CAMERA_RX = 16, CAMERA_TX = 17;
constexpr uint8_t MOTOR_SPEED = 150;
constexpr int OBSTACLE_LIMIT_CM = 25;

Adafruit_MCP23X17 mcp;
DHT dht(DHT_PIN, DHT22);
HardwareSerial cameraSerial(2);

RoverMode mode = STOPPED;
unsigned long lastAutonomyMs = 0, lastTelemetryMs = 0;

void setDirections(bool leftForward, bool rightForward) {
  mcp.digitalWrite(0, leftForward);  mcp.digitalWrite(1, !leftForward);
  mcp.digitalWrite(2, leftForward);  mcp.digitalWrite(3, !leftForward);
  mcp.digitalWrite(4, rightForward); mcp.digitalWrite(5, !rightForward);
  mcp.digitalWrite(6, rightForward); mcp.digitalWrite(7, !rightForward);
}

void setMotorPwm(uint8_t leftSpeed, uint8_t rightSpeed) {
  ledcWrite(LEFT_A_PWM, leftSpeed);  ledcWrite(LEFT_B_PWM, leftSpeed);
  ledcWrite(RIGHT_A_PWM, rightSpeed); ledcWrite(RIGHT_B_PWM, rightSpeed);
}

void drive(int leftSpeed, int rightSpeed) {
  setDirections(leftSpeed >= 0, rightSpeed >= 0);
  setMotorPwm(abs(leftSpeed), abs(rightSpeed));
  digitalWrite(LEFT_STBY, HIGH);
  digitalWrite(RIGHT_STBY, HIGH);
}

void emergencyStop() {
  setMotorPwm(0, 0);
  digitalWrite(LEFT_STBY, LOW);
  digitalWrite(RIGHT_STBY, LOW);
  mode = STOPPED;
}

long readDistanceCm() {
  digitalWrite(ULTRASONIC_TRIG, LOW); delayMicroseconds(3);
  digitalWrite(ULTRASONIC_TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  unsigned long duration = pulseIn(ULTRASONIC_ECHO, HIGH, 30000);
  return duration ? duration / 58 : -1;
}

void lineFollowStep() {
  bool lo = digitalRead(LINE_LEFT_OUTER) == LOW;
  bool li = digitalRead(LINE_LEFT_INNER) == LOW;
  bool ri = digitalRead(LINE_RIGHT_INNER) == LOW;
  bool ro = digitalRead(LINE_RIGHT_OUTER) == LOW;
  if (li && ri) drive(MOTOR_SPEED, MOTOR_SPEED);
  else if (lo || li) drive(80, MOTOR_SPEED);
  else if (ro || ri) drive(MOTOR_SPEED, 80);
  else emergencyStop();
}

void obstacleAvoidStep() {
  long distance = readDistanceCm();
  if (distance > 0 && distance < OBSTACLE_LIMIT_CM) {
    drive(-130, -130); delay(250);
    drive(150, -150); delay(380);
  } else drive(MOTOR_SPEED, MOTOR_SPEED);
}

void sendTelemetry() {
  float humidity = dht.readHumidity();
  float temperature = dht.readTemperature();
  long distance = readDistanceCm();
  int gasRaw = analogRead(GAS_ADC);
  Serial.printf("TELEMETRY mode=%d distance_cm=%ld temp_c=%.1f humidity_pct=%.1f gas_raw=%d\n", mode, distance, temperature, humidity, gasRaw);
  cameraSerial.printf("STATUS mode=%d distance=%ld\n", mode, distance);
}

void handleCommand(String command) {
  command.trim(); command.toUpperCase();
  if (command == "STOP") emergencyStop();
  else if (command == "MANUAL") { emergencyStop(); mode = MANUAL; }
  else if (command == "LINE") mode = LINE_FOLLOW;
  else if (command == "AVOID") mode = AVOID;
  else if (command == "FWD" && mode == MANUAL) drive(MOTOR_SPEED, MOTOR_SPEED);
  else if (command == "REV" && mode == MANUAL) drive(-MOTOR_SPEED, -MOTOR_SPEED);
  else if (command == "LEFT" && mode == MANUAL) drive(-MOTOR_SPEED, MOTOR_SPEED);
  else if (command == "RIGHT" && mode == MANUAL) drive(MOTOR_SPEED, -MOTOR_SPEED);
  else if (command == "CAM_ON") cameraSerial.println("STREAM_ON");
  else if (command == "CAM_OFF") cameraSerial.println("STREAM_OFF");
}

void pollCommands(Stream &port, String &buffer) {
  while (port.available()) {
    char c = static_cast<char>(port.read());
    if (c == '\n' || c == '\r') { if (buffer.length()) handleCommand(buffer); buffer = ""; }
    else if (buffer.length() < 48) buffer += c;
  }
}

void setup() {
  Serial.begin(115200);
  cameraSerial.begin(115200, SERIAL_8N1, CAMERA_RX, CAMERA_TX);
  Wire.begin(21, 22);
  if (!mcp.begin_I2C(0x20, &Wire)) {
    Serial.println("FATAL: MCP23017 not found; motors remain locked.");
    while (true) delay(1000);
  }
  for (uint8_t pin = 0; pin < 8; ++pin) { mcp.pinMode(pin, OUTPUT); mcp.digitalWrite(pin, LOW); }
  pinMode(LEFT_STBY, OUTPUT); pinMode(RIGHT_STBY, OUTPUT);
  pinMode(ULTRASONIC_TRIG, OUTPUT); pinMode(ULTRASONIC_ECHO, INPUT);
  pinMode(LINE_LEFT_OUTER, INPUT); pinMode(LINE_LEFT_INNER, INPUT);
  pinMode(LINE_RIGHT_INNER, INPUT); pinMode(LINE_RIGHT_OUTER, INPUT);
  ledcAttach(LEFT_A_PWM, 18000, 8); ledcAttach(LEFT_B_PWM, 18000, 8);
  ledcAttach(RIGHT_A_PWM, 18000, 8); ledcAttach(RIGHT_B_PWM, 18000, 8);
  dht.begin(); emergencyStop();
  Serial.println("Ready: STOP, MANUAL, FWD, REV, LEFT, RIGHT, LINE, AVOID, CAM_ON, CAM_OFF");
}

void loop() {
  static String usbBuffer, cameraBuffer;
  pollCommands(Serial, usbBuffer); pollCommands(cameraSerial, cameraBuffer);
  unsigned long now = millis();
  if (now - lastAutonomyMs >= 80) {
    lastAutonomyMs = now;
    if (mode == LINE_FOLLOW) lineFollowStep();
    if (mode == AVOID) obstacleAvoidStep();
  }
  if (now - lastTelemetryMs >= 3000) { lastTelemetryMs = now; sendTelemetry(); }
}