#include <ESP32Servo.h>

const int trigPin = 32;
const int echoPin = 33;
const int servoPin = 21;

const int LED1 = 14;
const int LED2 = 15;
const int LED3 = 16;
const int LED4 = 17;
const int LED5 = 18;

int duration = 0;
int distance = 0;

Servo myservo;

void setup() 
{
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  pinMode(servoPin, OUTPUT);
  
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  pinMode(LED4, OUTPUT);
  pinMode(LED5, OUTPUT);
  
  myservo.attach(servoPin);
  myservo.write(0);
  
  Serial.begin(9600);
}

void loop()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration / 58.2;

  if (distance <= 30)
  {
    digitalWrite(LED1, LOW);
    digitalWrite(LED2, LOW);
    digitalWrite(LED3, LOW);
    digitalWrite(LED4, LOW);
    digitalWrite(LED5, LOW);
    
    myservo.write(0);
  }
  else
  {
    if (distance <= 50)
    {
      digitalWrite(LED1, HIGH);
    }
    else
    {
      digitalWrite(LED1, LOW);
    }
    if (distance <= 100)
    {
      digitalWrite(LED2, HIGH);
    }
    else
    {
      digitalWrite(LED2, LOW);
    }
    if (distance <= 200)
    {
      digitalWrite(LED3, HIGH);
    }
    else
    {
      digitalWrite(LED3, LOW);
    }
    if (distance <= 300)
    {
      digitalWrite(LED4, HIGH);
    }
    else
    {
      digitalWrite(LED4, LOW);
    }
    if (distance <= 400)
    {
      digitalWrite(LED5, HIGH);
      myservo.write(90);
      
    }
    else
    {
      digitalWrite(LED5, LOW);
      myservo.write(0);
     
    }
  }
 
  delay(100);
}