#include <Servo.h>  // Include the Servo library to control the servo motor

Servo myservo;  // Create a servo object to control a servo motor

const int trigPin = 2;  // Define pin for ultrasonic sensor trigger
const int echoPin = 13;  // Define pin for ultrasonic sensor echo
float distance, duration;  // Variables to store distance and duration of the ultrasonic pulse
// Motor pins for Motor1
const int en1 = 3;
const int MotorPin1 = 12; // Motor1 forward
const int MotorPin2 = 10; // Motor1 backward

// Motor pins for Motor2
const int en2 = 11;
const int MotorPin3 = 9;  // Motor2 forward
const int MotorPin4 = 6;  // Motor2 backward

void setup() {
  // Set motor pins as OUTPUT for controlling motors
  pinMode(MotorPin1, OUTPUT);
  pinMode(MotorPin2, OUTPUT);
  pinMode(MotorPin3, OUTPUT);
  pinMode(MotorPin4, OUTPUT);
  pinMode(en1, OUTPUT);
  pinMode(en2, OUTPUT);

  myservo.attach(5);  // Attach the servo to pin 5 to control it
  pinMode(trigPin, OUTPUT);  // Set the trigPin as an Output
  pinMode(echoPin, INPUT);  // Set the echoPin as an Input
  Serial.begin(9600);  // Start serial communication at 9600 baud rate for debugging
}

void loop() {
  analogWrite(en1, 180);
	analogWrite(en2, 180);
  ultrason();  // Call the ultrason function to get the distance
  Serial.println("The distance detected by the ultrasonic sensor: ");
  Serial.print(distance);  // Print the distance detected by the ultrasonic sensor
  delay(1000);  // Wait for 1 second before the next measurement
}

void motor_stop() {
  // Stop both motors by setting all motor control pins to LOW
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, LOW);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, LOW);
}

void motor_forward() {
  // Move the robot forward by rotating the motors in the forward direction
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, HIGH);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, HIGH);
}

void motor_backward() {
  // Move the robot backward by rotating the motors in the backward direction
  digitalWrite(MotorPin1, HIGH);
  digitalWrite(MotorPin2, LOW);
  digitalWrite(MotorPin3, HIGH);
  digitalWrite(MotorPin4, LOW);
}

void motor_right() {
  // Turn the robot right by rotating the motors in a right-turn direction
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, HIGH);
  digitalWrite(MotorPin3, HIGH);
  digitalWrite(MotorPin4, LOW);
}

void motor_left() {
  // Turn the robot left by rotating the motors in a left-turn direction
  digitalWrite(MotorPin1, HIGH);
  digitalWrite(MotorPin2, LOW);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, HIGH);
}

void ultrason() {
  // Send a pulse to the ultrasonic sensor to measure distance
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);  // Wait for 2 microseconds before sending the pulse
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);  // Send a 10-microsecond pulse
  digitalWrite(trigPin, LOW); 

  // Measure the pulse duration using echoPin
  duration = pulseIn(echoPin, HIGH);  // Capture the pulse duration
  distance = duration * 0.034 / 2;  // Calculate the distance based on the pulse duration

  // Print the distance measured by the ultrasonic sensor
  Serial.print("The distance: ");
  Serial.println(distance);

  // Check if the distance is less than or equal to 5 cm (obstacle detected)
  if (distance <= 5) {
    motor_stop();  // Stop the robot if an obstacle is detected
    Serial.print("Obstacle detected");
  } else {
    motor_forward();  // Move the robot forward if no obstacle is detected
  }

  return distance;  // Return the measured distance
}