#include <Servo.h>  

#define SERVO_PIN 9  
#define LED_PIN 13   
#define TRIG_PIN 3    
#define ECHO_PIN 2    

Servo servo; 
int pos = 0; 
bool firstRun = true;  // Variable to track if it's the first loop iteration

void setup() {
  pinMode(LED_PIN, OUTPUT);  
  pinMode(TRIG_PIN, OUTPUT); 
  pinMode(ECHO_PIN, INPUT);  
  servo.attach(SERVO_PIN);  
  Serial.begin(9600);  // Initialize serial communication for debugging
}

void loop() {
 
  // Trigger the ultrasonic sensor to send out a pulse
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Read the pulse duration from the sensor
  float duration_us = pulseIn(ECHO_PIN, HIGH);

  // Calculate the distance in cm
  float distance_cm = 0.017 * duration_us;

  Serial.print("Distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  int targetPos;
  
  if (distance_cm < 20) {
    targetPos = 45;  // Set position to 45 degrees

    delay(200);  // 1-second delay before moving the servo from 0 to 45 degrees
  } else {
    targetPos = 180; // Set position to 180 degrees
  }

  digitalWrite(LED_PIN, HIGH);  // Turn on the LED while moving

  // Move the servo from its current position to the target position
  if (targetPos < pos) {
    for (pos = pos; pos >= targetPos; pos--) {
      servo.write(pos); 
      delay(10);        
    }
  } else {
    for (pos = pos; pos <= targetPos; pos++) {
      servo.write(pos); 
      delay(10);        
    }
  }

  digitalWrite(LED_PIN, LOW);  // Turn off the LED when done moving
  delay(500);  // Wait before the next loop iteration
}