#include <ESP32Servo.h>

#define trig 25
#define echo 26
#define servo 14
#define ver 19
#define azul 18
#define lara 5
#define rosa 17
#define verd 16

Servo servom;

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  servom.attach(servo);
  
  pinMode(ver, OUTPUT);
  pinMode(azul, OUTPUT);
  pinMode(lara, OUTPUT);
  pinMode(rosa, OUTPUT);
  pinMode(verd, OUTPUT);
}

void loop() {
  long pulso, distancia;
  digitalWrite(trig, LOW);  
  delayMicroseconds(2); 
  digitalWrite(trig, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trig, LOW);
  pulso = pulseIn(echo, HIGH);
  distancia = 0.017 * pulso;
  
  if (distancia > 0 && distancia <= 100) {
    digitalWrite(ver, HIGH);
    digitalWrite(azul, HIGH);
    digitalWrite(lara, HIGH);
  } 
  else if (distancia > 100 && distancia <= 200) {
    digitalWrite(ver, HIGH);
    digitalWrite(azul, HIGH);
    digitalWrite(lara, HIGH);
    digitalWrite(rosa, HIGH);
    digitalWrite(verd, HIGH);
  } 
  else if (distancia > 200 && distancia <= 400) {
    for (int pos = 0; pos <= 90; pos += 1) {    
    servom.write(pos);
    delay(15);
    }
    for (int pos = 90; pos >= 0; pos -= 1) {
    servom.write(pos);
    delay(15);
    }
  }
}