#include <ESP32Servo.h>                  // SE DEFINE AL SERVOMOTOR. 

Servo miservo;
int pos;
//int puenteR=9;
//int puenteL=8;
int motorRP=13;
int motorRB=12;
int motorLP=14;
int motorLB=27;
unsigned long pulso;
float distancia;
float derecha;
float izquierda;

int trig=18;
 int echo=5;
int duracion; 
void setup() {
  // put your setup code here, to run once:
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
//pinMode(puenteR, OUTPUT);
//pinMode(puenteL, OUTPUT);
pinMode(motorRP, OUTPUT);
pinMode(motorRB, OUTPUT);
pinMode(motorLP, OUTPUT);
pinMode(motorLB, OUTPUT);

//digitalWrite(puenteR,LOW);
//digitalWrite(puenteL,LOW);

miservo.attach(2);
Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
digitalWrite(trig,HIGH);
delayMicroseconds(100);
digitalWrite(trig, LOW);

duracion= pulseIn(echo, HIGH);
distancia = duracion/58.2;
Serial.println(distancia);
delay(100);

if(distancia < 35){
  digitalWrite(motorRP, LOW);
  digitalWrite(motorRB, LOW);
  //digitalWrite(puenteR, LOW);
  //digitalWrite(puenteL, LOW);
  digitalWrite(motorLP, LOW);
  digitalWrite(motorLB, LOW);

  miservo.write(pos=0);
  digitalWrite(trig, HIGH);
  delayMicroseconds(100);
  digitalWrite(trig, LOW);
  duracion= pulseIn(echo,HIGH);
  derecha = duracion/58.2;
  delay(1000+derecha);
  Serial.print("derecha");
  Serial.print(derecha);

  miservo.write(pos=180);
  digitalWrite(trig,HIGH);
  delayMicroseconds(100);
  digitalWrite(trig, LOW);
  duracion= pulseIn(echo,HIGH);
  izquierda = duracion/58.2;
  delay(1000+izquierda);
   //Frente
  miservo.write(pos=90);
  digitalWrite(trig,HIGH);
  delayMicroseconds(100);
  digitalWrite(trig, LOW);
  duracion= pulseIn(echo,HIGH);
  distancia = duracion/58.2;
  delay(1000);
   //
   digitalWrite(motorRP,HIGH);
   digitalWrite(motorRB,LOW);
  //digitalWrite(puenteR,HIGH);
 // digitalWrite(puenteL,HIGH);
  digitalWrite(motorLP,HIGH);
   digitalWrite(motorLB,LOW);
   delay(400);

   if(derecha>izquierda) {
      digitalWrite(motorRP,HIGH);
   digitalWrite(motorRB,LOW);
  //digitalWrite(puenteR,HIGH);
  //digitalWrite(puenteL,HIGH);
  digitalWrite(motorLP,LOW);
   digitalWrite(motorLB,HIGH);
delay(400);
    
    }
else{
  digitalWrite(motorRP,LOW);
   digitalWrite(motorRB,HIGH);
  //digitalWrite(puenteR,HIGH);
  //digitalWrite(puenteL,HIGH);
  digitalWrite(motorLP,HIGH);
   digitalWrite(motorLB,LOW);
   delay(400);
}
    
   }

   else{
    miservo.write(pos=90);
    digitalWrite(trig, HIGH);
    delayMicroseconds(100);
    digitalWrite(trig,LOW);
    duracion= pulseIn(echo, HIGH);
    distancia= duracion/58.2;
    digitalWrite(motorRP,LOW);
   digitalWrite(motorRB,HIGH);
  //digitalWrite(puenteR,HIGH);
  //digitalWrite(puenteL,HIGH);
  digitalWrite(motorLP,LOW);
   digitalWrite(motorLB,HIGH);
   delay(1000); 
    
   }
}