#include <Servo.h>

Servo M1;
Servo M2;

const int trigPin = 9;  // Trig pin of the ultrasonic sensor
const int echoPin = 10; // Echo pin of the ultrasonic sensor


void setup() {
  Serial.begin(9600); // Initialize serial communication
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  M1.attach(5);
  M2.attach(6);

  M1.write(120);
  M2.write(120);
  delay(2000);
  M1.write(60);
  M2.write(60);
  delay(2000);
  M1.write(0);
  M2.write(0);
  delay(2000);
}

void loop() {
  long duration;
  int distance;

  
  // Clear the trigger pin to ensure a clean pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Send a 10 microsecond pulse to the trigger pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echo pin, and calculate distance based on the duration of the pulse
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; // Speed of sound is 34,000 cm/s (divided by 2 because it's a round trip)

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

  // Check if the object is under or over 20cm away
  if (distance < 20) {
    Serial.println("Object is under 20cm away");
    for(int i = 0; i<40; i++){ // Set the servo angle to 40 degrees
      M1.write(i);
      delay(80);
    }
      
    for(int q = 0; q<50; q++){
      M2.write(q);
      delay(20);
    }
    M2.write(0);
    // Your code for actions when an object is under 20cm away
  } 
  else {
    Serial.println("Object is 20cm or more away");
    // Your code for actions when an object is 20cm or more away
  }

  delay(1000); // Delay between distance measurements
}