#include<ESP32Servo.h>
Servo s0;
int pinServo=21;    //Pin servo
int pos=0;    //inicia en 0 grados


int trigger = 19;   //Pin Trigger
int echo = 18;    //Pin Echo
float tiempo_ida_vuelta = 0;
float distancia = 0;

int c=0;
int a=0;
int der=0;
int izq=0;


void setup() {
  pinMode(5,OUTPUT);    //motor1 adelante   //Cambiar los pines aqui y abajo
  pinMode(6,OUTPUT);    //motor1 atras
  pinMode(9,OUTPUT);    //motor2 adelante
  pinMode(10,OUTPUT);   //motor2 atras

  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT);

  s0.attach(pinServo, 500, 2500);   //Inicia posicion de servo
  Serial.begin(115200);
}

void loop() {
  
  digitalWrite(trigger, HIGH);
  delay(1);
  digitalWrite(trigger, LOW);
  tiempo_ida_vuelta = pulseIn(echo, HIGH);

  distancia = tiempo_ida_vuelta / 58.2;   //distancia en cm
    Serial.print("Dist: ");
    Serial.print(distancia);
    Serial.println("  cm");


  //------Caso avance recto------
  if(distancia >= 5){    //Dist mayor a 5cm
                
                if(der == 0 && izq == 0){
                //------Hacia delante------
                digitalWrite(5,HIGH);
                digitalWrite(6,LOW);
                digitalWrite(9,HIGH);
                digitalWrite(10,LOW);
                delay(100);
                }

                else if(der==1){

                      //------Hacia derecha------
                      digitalWrite(5,HIGH);
                      digitalWrite(6,LOW);
                      digitalWrite(9,LOW);
                      digitalWrite(10,HIGH);
                      delay(3000);   //Tiempo de giro VER CUANTO SE DEMORA EN GIRAR
                      //------Apago------
                      digitalWrite(5,LOW);
                      digitalWrite(6,LOW);
                      digitalWrite(9,LOW);
                      digitalWrite(10,LOW);
                      der=0;
                }
                
                else if(izq==1){
                  
                      //------Hacia izquierda------
                      digitalWrite(5,LOW);
                      digitalWrite(6,HIGH);
                      digitalWrite(9,HIGH);
                      digitalWrite(10,LOW);
                      delay(3000);   //Tiempo de giro VER CUANTO SE DEMORA EN GIRAR
                      //------Apago------
                      digitalWrite(5,LOW);
                      digitalWrite(6,LOW);
                      digitalWrite(9,LOW);
                      digitalWrite(10,LOW);
                      izq=0;
                }
                s0.write(90);   //Importante la posicion de esta linea
                c=0;  a=0;
  }
  //------Caso detecta obstaculo frente------
  else if(distancia <= 5 && distancia >= 0){   //Dist menor a 5cm (Mayor a cero, porque si se sale del rango maximo del sensor 450 cm salen valores negativos)
    
    //------Apago------
                      digitalWrite(5,LOW);
                      digitalWrite(6,LOW);
                      digitalWrite(9,LOW);
                      digitalWrite(10,LOW);

    //Primero esta detectando a la derecha. Si hay ambos caminos libres, primero irá a la derecha
    if(c==0){
                  //Girar servo derecha
                  s0.write(10);   //Estos angulos pueden cambiar o mejor cambiar la posicion del servo
                  delay(2000);
                  a=a+1;
                  der=1;
                  izq=0;
    }  
    else if(c==1){
                  //Girar servo izquierda
                  s0.write(170);
                  delay(2000);
                  a=a+1;
                  der=0;
                  izq=1;
    }
    else if(a==2){
                  //------Hacia atras------
                  digitalWrite(5,LOW);
                  digitalWrite(6,HIGH);
                  digitalWrite(9,LOW);
                  digitalWrite(10,HIGH);
                  delay(100);
    }
    c=1-c;    //Cada que pase va a cambiar de 0 a 1
  }
}




  // if(dato=='1')   //Hacia delante
  // {
  //   digitalWrite(5,HIGH);
  //   digitalWrite(6,LOW);
  //   digitalWrite(9,HIGH);
  //   digitalWrite(10,LOW);
  //   delay(100);
  // }
  // if(dato=='4')   //Hacia izquierda
  // {
  //   digitalWrite(5,LOW);
  //   digitalWrite(6,HIGH);
  //   digitalWrite(9,HIGH);
  //   digitalWrite(10,LOW);
  //   delay(100);
  // }
  // if(dato=='3')   //Hacia atras
  // {
  //   digitalWrite(5,LOW);
  //   digitalWrite(6,HIGH);
  //   digitalWrite(9,LOW);
  //   digitalWrite(10,HIGH);
  //   delay(100);
  // }
  // if(dato=='2')   //Hacia derecha
  // {
  //   digitalWrite(5,HIGH);
  //   digitalWrite(6,LOW);
  //   digitalWrite(9,LOW);
  //   digitalWrite(10,HIGH);
  //   delay(100);
  // }
  // if(dato=='0')   //Quieto
  // {
  //   digitalWrite(5,LOW);
  //   digitalWrite(6,LOW);
  //   digitalWrite(9,LOW);
  //   digitalWrite(10,LOW);
  //   delay(100);
  // }
  // if(dato=='a')
  // {
  //   s0.attach(3);
  //   s0.write(90);
  //   delay(100);
  //   s0.write(20);
  //   delay(100);
  //   s0.write(90);
  //   delay(100);
  //   s0.detach();
  // }
// }