Autonomous Waste-Sorting Robot

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_PWMServoDriver.h>

enum RobotState { FOLLOWING, STOPPED, SORTING };


// Forward declarations
void setMotors(int leftForward, int leftReverse, int rightForward, int rightReverse);
void stopMotors();
void driveForward();
void turnLeft();
void turnRight();
float readDistanceCm();
void drawStatus(float distanceCm, int moisture, bool metalDetected);
void tipBin(uint8_t channel, const char *label);
void followLine();

constexpr int SONAR_TRIG_PIN = 25;
constexpr int SONAR_ECHO_PIN = 26;
constexpr int LEFT_IN1_PIN = 16;
constexpr int LEFT_IN2_PIN = 17;
constexpr int RIGHT_IN1_PIN = 18;
constexpr int RIGHT_IN2_PIN = 19;
constexpr int MOISTURE_PIN = 34;
constexpr int LINE_S1_PIN = 4;
constexpr int LINE_S2_PIN = 13;
constexpr int LINE_S3_PIN = 14;
constexpr int LINE_S4_PIN = 27;
constexpr int LINE_S5_PIN = 33;
constexpr int METAL_SENSOR_PIN = 32;
constexpr int I2C_SDA_PIN = 21;
constexpr int I2C_SCL_PIN = 22;

constexpr uint8_t SCREEN_WIDTH = 128;
constexpr uint8_t SCREEN_HEIGHT = 64;
constexpr uint8_t OLED_ADDRESS = 0x3C;
constexpr uint8_t PCA9685_ADDRESS = 0x40;
constexpr uint8_t WET_SERVO_CHANNEL = 0;
constexpr uint8_t DRY_SERVO_CHANNEL = 1;
constexpr uint8_t METAL_SERVO_CHANNEL = 2;
constexpr uint16_t SERVO_CLOSED_PULSE = 180;
constexpr uint16_t SERVO_TIP_PULSE = 430;
constexpr int MOTOR_SPEED = 185;
constexpr int TURN_SPEED = 160;
constexpr float STOP_DISTANCE_CM = 10.0f;
constexpr int WET_THRESHOLD = 2100;
constexpr unsigned long STATUS_PERIOD_MS = 250;
constexpr unsigned long SORT_COOLDOWN_MS = 3000;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_PWMServoDriver servoDriver(PCA9685_ADDRESS);
RobotState robotState = FOLLOWING;
String statusLine = "Starting";
unsigned long lastStatusUpdate = 0;
unsigned long lastSortTime = 0;

void setMotors(int leftForward, int leftReverse, int rightForward, int rightReverse) {
  ledcWrite(0, leftForward);
  ledcWrite(1, leftReverse);
  ledcWrite(2, rightForward);
  ledcWrite(3, rightReverse);
}

void stopMotors() { setMotors(0, 0, 0, 0); }
void driveForward() { setMotors(MOTOR_SPEED, 0, MOTOR_SPEED, 0); }
void turnLeft() { setMotors(0, TURN_SPEED, TURN_SPEED, 0); }
void turnRight() { setMotors(TURN_SPEED, 0, 0, TURN_SPEED); }

float readDistanceCm() {
  digitalWrite(SONAR_TRIG_PIN, LOW);
  delayMicroseconds(3);
  digitalWrite(SONAR_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(SONAR_TRIG_PIN, LOW);
  unsigned long pulse = pulseIn(SONAR_ECHO_PIN, HIGH, 25000);
  return pulse == 0 ? 999.0f : pulse * 0.0343f / 2.0f;
}

void drawStatus(float distanceCm, int moisture, bool metalDetected) {
  if (millis() - lastStatusUpdate < STATUS_PERIOD_MS) return;
  lastStatusUpdate = millis();
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("Swachh-Bot 360");
  display.print("Mode: "); display.println(statusLine);
  display.print("Range: ");
  if (distanceCm >= 999.0f) display.println("--");
  else { display.print(distanceCm, 0); display.println(" cm"); }
  display.print("Moisture: "); display.println(moisture);
  display.print("Metal: "); display.println(metalDetected ? "YES" : "NO");
  display.display();
}

void tipBin(uint8_t channel, const char *label) {
  robotState = SORTING;
  statusLine = label;
  stopMotors();
  servoDriver.setPWM(channel, 0, SERVO_TIP_PULSE);
  delay(1100);
  servoDriver.setPWM(channel, 0, SERVO_CLOSED_PULSE);
  delay(500);
  lastSortTime = millis();
  statusLine = "Following line";
  robotState = FOLLOWING;
}

void followLine() {
  // Most line modules output LOW on black. Remove ! if your module outputs HIGH on black.
  bool leftOuter = !digitalRead(LINE_S1_PIN);
  bool leftInner = !digitalRead(LINE_S2_PIN);
  bool centre = !digitalRead(LINE_S3_PIN);
  bool rightInner = !digitalRead(LINE_S4_PIN);
  bool rightOuter = !digitalRead(LINE_S5_PIN);
  if (centre || (leftInner && rightInner)) driveForward();
  else if (leftOuter || leftInner) turnLeft();
  else if (rightOuter || rightInner) turnRight();
  else { stopMotors(); statusLine = "Line lost"; }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  pinMode(SONAR_TRIG_PIN, OUTPUT);
  pinMode(SONAR_ECHO_PIN, INPUT);
  pinMode(LINE_S1_PIN, INPUT);
  pinMode(LINE_S2_PIN, INPUT);
  pinMode(LINE_S3_PIN, INPUT);
  pinMode(LINE_S4_PIN, INPUT);
  pinMode(LINE_S5_PIN, INPUT);
  pinMode(METAL_SENSOR_PIN, INPUT);

  ledcAttachChannel(LEFT_IN1_PIN, 20000, 8, 0);
  ledcAttachChannel(LEFT_IN2_PIN, 20000, 8, 1);
  ledcAttachChannel(RIGHT_IN1_PIN, 20000, 8, 2);
  ledcAttachChannel(RIGHT_IN2_PIN, 20000, 8, 3);
  stopMotors();

  servoDriver.begin();
  servoDriver.setOscillatorFrequency(27000000);
  servoDriver.setPWMFreq(50);
  servoDriver.setPWM(WET_SERVO_CHANNEL, 0, SERVO_CLOSED_PULSE);
  servoDriver.setPWM(DRY_SERVO_CHANNEL, 0, SERVO_CLOSED_PULSE);
  servoDriver.setPWM(METAL_SERVO_CHANNEL, 0, SERVO_CLOSED_PULSE);

  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) Serial.println("OLED not found");
  statusLine = "Following line";
}

void loop() {
  float distanceCm = readDistanceCm();
  int moisture = analogRead(MOISTURE_PIN);
  bool metalDetected = !digitalRead(METAL_SENSOR_PIN);

  if (robotState == FOLLOWING && distanceCm > STOP_DISTANCE_CM) {
    statusLine = "Following line";
    followLine();
  } else if (robotState == FOLLOWING) {
    stopMotors();
    statusLine = "Object detected";
    robotState = STOPPED;
  }

  if (robotState == STOPPED && millis() - lastSortTime > SORT_COOLDOWN_MS) {
    if (metalDetected) tipBin(METAL_SERVO_CHANNEL, "METAL CANS");
    else if (moisture > WET_THRESHOLD) tipBin(WET_SERVO_CHANNEL, "WET ORGANIC");
    else tipBin(DRY_SERVO_CHANNEL, "DRY RECYCLABLE");
  }

  drawStatus(distanceCm, moisture, metalDetected);
  delay(15);
}