// UltraSonicos
const int TriggerIz=A0;           //  
const int EchoIz=A1;              //  
const int TriggerFr=A2;           //  
const int EchoFr=A3;              //  
const int TriggerDr=A4;           //  
const int EchoDr=A5;              //  
// Motor A    
const int  ENA = 6;               //  
const int  IN1 = 2;               //  
const int  IN2 = 3;               //  
// Motor B
const int  ENB = 9;               //   
const int  IN3 = 4;               //  
const int  IN4 = 5;               //  

// otros pines
const int  buzzer = 8;            //  
const int  led1Pos = 10;          //  
const int  led2Pos = 11;          //  
const int  led3Fr = 12;           //  
const int  led4Fr = 13;           //  
void setup ()
{
  // Declaramos todos los pines del ultrasonido como entradas 0 salidas según sea el caso
  pinMode(TriggerIz, OUTPUT);     //  
  pinMode(TriggerFr, OUTPUT);     //  
  pinMode(TriggerDr, OUTPUT);     //  
  pinMode(EchoIz, INPUT);         //  
  pinMode(EchoFr, INPUT);         //  
  pinMode(EchoDr, INPUT);         //  
  // Declaramos todos los pines del Driver L298N ultrasonido como salidas
  pinMode (ENA, OUTPUT);          //  
  pinMode (ENB, OUTPUT);          //  
  pinMode (IN1, OUTPUT);          //  
  pinMode (IN2, OUTPUT);          //  
  pinMode (IN3, OUTPUT);          //  
  pinMode (IN4, OUTPUT);          //  
  // Declaramos el resto de pines como salidas
  //pinMode (buzzer, OUTPUT);
  pinMode (led1Pos, OUTPUT);      //  
  pinMode (led2Pos, OUTPUT);      //  
  pinMode (led3Fr, OUTPUT);       //  
  pinMode (led4Fr, OUTPUT);       //  
  // Configuración del Puerto serial  
  Serial.begin(9600);             //  
}
void loop() {
  // declaración de variables de distancia
  int distanciaiz;                //  
  int distanciafr;                //  
  int distanciadr;                //  
  // declaración de variables de tiempo
  int tiempoiz;                   //  
  int tiempofr;                   //  
  int tiempodr;                   //  
  // Código distancia Izquierda
  digitalWrite(TriggerIz, LOW);
  digitalWrite(TriggerIz, HIGH);
  delayMicroseconds(10);
  digitalWrite(TriggerIz, LOW);
  tiempoiz=pulseIn(EchoIz, HIGH);
  distanciaiz=tiempoiz*0.0171;
  Serial.println(distanciaiz);
  delay(100);
  // Código distancia Frontal
  digitalWrite(TriggerFr, LOW);
  digitalWrite(TriggerFr, HIGH);
  delayMicroseconds(10);
  digitalWrite(TriggerFr, LOW);
  tiempofr=pulseIn(EchoFr, HIGH);
  distanciafr=tiempofr*0.0171;
  Serial.println(distanciafr);
  delay(100);
  // Código distancia Derecha
  digitalWrite(TriggerDr, LOW);
  digitalWrite(TriggerDr, HIGH);
  delayMicroseconds(10);
  digitalWrite(TriggerDr, LOW);
  tiempodr=pulseIn(EchoDr, HIGH);
  distanciadr=tiempodr*0.0171;
  Serial.println(distanciadr);
  delay(100);

  // código laberinto ejemplo
          //         ejemplo 2 condiciones
  if ((distanciaiz < 8) && (distanciafr < 8)) {           // por verificar distancia
      stop();
      delay(500);
      derecha ();
      stop();
      delay(500);
    }
  else{
    stop();
    delay(500);
    izquierda();
    stop();
    delay(500);
  }
  if ((distanciadr < 8) || (distanciafr < 8)) {
    stop();
    delay(300);
    izquierda();
    stop();
    delay(200);
  }
    else  {
    delay(300);
    derecha();
    stop();
    delay(200);
    }
  
          //          ejemplo 3 condiciones
  if ((distanciaiz < 8) && (distanciafr < 8) && (distanciadr < 8)) { // por verificar distancia
      stop();
      delay(300);
      giro180();
      stop();
      delay(200);
    }
    else  {
      stop();
      delay(300);
      atras();
      delay(500);
  }
}
// Declaración de las funciones para el movimiento de vehículo
void atras()      {                      //     led rojo=on y led azul=on
  analogWrite(ENA,120);                 //
  digitalWrite(IN1, LOW);           //
  digitalWrite(IN2, HIGH);          //
  analogWrite(ENB,125);             //
  digitalWrite(IN3, LOW);           //
  digitalWrite(IN4, HIGH);          //
}
void adelante()   {                      //     led verde=on y led naranja=on
  analogWrite(ENA,123);             //
  digitalWrite(IN1, HIGH);          //
  digitalWrite(IN2, LOW);           //
  analogWrite(ENB,132);             //
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void derecha()    {                      //     led verde=on y led azul=on  girar 90 grados
  analogWrite(ENA,122);             //
  digitalWrite(IN1, LOW);           //
  digitalWrite(IN2, HIGH);          //
  analogWrite(ENB,112);             //
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  delay(125);                       //  por confirmar el valor del tiempo para el giro
}
void izquierda()  {                      //     led rojo=on y led naranja=on
  analogWrite(ENA,115);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENB,110);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  delay(115);                     //  por confirmar el valor del tiempo para el giro
}
void stop()       {
  analogWrite(ENA,0);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  analogWrite(ENB,0);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  delay(500);
}
void giro180()    {            
  analogWrite(ENA,113);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENB,119);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  delay(450);                 //  por confirmar el valor del tiempo para el giro
}