#include <ESP32Servo.h>

 Servo myservo;

const int Trigger = 25;   //Pin digital 2 para el Trigger del sensor
const int Echo = 26;   //Pin digital 3 para el Echo del sensor
int estado;
int tiempo;


void setup() {

  Serial.begin(9600);//iniciailzamos la comunicación
  pinMode(Trigger, OUTPUT); //pin como salida
  pinMode(Echo, INPUT);  //pin como entrada
  digitalWrite(Trigger, LOW);//Inicializamos el pin con 0
  pinMode(27, OUTPUT);
  digitalWrite(27, LOW);  
  estado=0;
  tiempo=millis();
 }


void loop(){
  
  long t; //timepo que demora en llegar el eco
  long d; //distancia en centimetros

  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);          //Enviamos un pulso de 10us
  digitalWrite(Trigger, LOW);
  
  t = pulseIn(Echo, HIGH); //obtenemos el ancho del pulso
  d = t/59;             //escalamos el tiempo a una distancia en cm
  if (estado==0){   
  tiempo=1000;
  }
  if(tiempo>1000){
  estado=1;
  }
  if(estado==1){
  if(d<=100){
    digitalWrite(27, HIGH);
     myservo.attach(32);
     myservo.attach(33);
     myservo.write(0);  // ajusta el servo al punto medio
   delay(500);
    myservo.attach(32);
    myservo.attach(33);
   myservo.write(180);  // ajusta el servo al punto medio
   delay(500);

  }
  }
  if(d<=20){
     myservo.attach(32);
     myservo.write(0);  // ajusta el servo al punto medio
     myservo.attach(33);
     myservo.write(180);  // ajusta el servo al punto medio
   delay(500);
    myservo.attach(32);
    myservo.write(0);  // ajusta el servo al punto medio
    myservo.attach(33);
   myservo.write(180);  // ajusta el servo al punto medio
   delay(500);


  }

  Serial.print("Distancia: ");
  Serial.print(d);      //Enviamos serialmente el valor de la distanci
  Serial.print("cm");
  Serial.println();
  delay(100);         
}