#include <NewPing.h>

#define trigpin 3
#define echopin 2
#define MAX_SPEED 255 // Max speed of DC motors
#define MAX_SPEED_OFFSET 20 // Speed adjustment offset

int ena = 5;  // Motor 1 speed pin (PWM)
int enb = 6;  // Motor 2 speed pin (PWM)

int m11 = 8;   // Motor 1 direction pin
int m12 = 9;   // Motor 1 direction pin
int m21 = 10;  // Motor 2 direction pin
int m22 = 11;  // Motor 2 direction pin

void setup() {
  Serial.begin(9600);

  pinMode(ena, OUTPUT);
  pinMode(enb, OUTPUT);

  pinMode(m11, OUTPUT);
  pinMode(m12, OUTPUT);

  pinMode(m21, OUTPUT);
  pinMode(m22, OUTPUT);

  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
}

void loop() {
  int duration, distance;

  // Send trigger pulse to the ultrasonic sensor
  digitalWrite(trigpin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigpin, LOW);

  // Measure the duration of the echo pulse
  duration = pulseIn(echopin, HIGH);
  
  // Calculate the distance in cm
  distance = (duration / 2) / 29.1;

  // Print the distance to the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);

  // Determine the motor speed based on the distance
  int speed = map(distance, 0, 30, MAX_SPEED, 0);  // Map the distance to speed (faster when closer)

  if (distance >= 50) {
    // Stop the motors if the distance is greater than or equal to 40 cm
    analogWrite(ena, 0);
    analogWrite(enb, 0);
    digitalWrite(m11, LOW);
    digitalWrite(m12, LOW);
    digitalWrite(m21, LOW);
    digitalWrite(m22, LOW);
    delay(500);  // Brief pause before restarting
  } else if (distance <= 14) {
    // Stop the motors if the distance is less than or equal to 10 cm (close to object)
    analogWrite(ena, 0);
    analogWrite(enb, 0);
    digitalWrite(m11, LOW);
    digitalWrite(m12, LOW);
    digitalWrite(m21, LOW);
    digitalWrite(m22, LOW);
    Serial.println("Object detected! Stopping.");
  } else {
    if (distance >= 15) {
      // Move forward with adjusted speed
      analogWrite(ena, speed);  // Set speed for Motor 1
      analogWrite(enb, speed);  // Set speed for Motor 2
      digitalWrite(m11, HIGH);  // Motor 1 forward
      digitalWrite(m12, LOW);
      digitalWrite(m21, HIGH);  // Motor 2 forward
      digitalWrite(m22, LOW);
      delay(100);
    }
  }
}