// 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);

  //adelante();

  // código laberinto.
          //         ejemplo 2 condiciones
  if ((distanciaiz < 10) || (distanciafr < 10)) {
      stop();
      delay(500);
      derecha ();
      stop();
      delay(500);
    }
  else{
    stop();
    delay(500);
    izquierda();
    stop();
    delay(500);
  }
          //          ejemplo 3 condiciones
  if ((distanciaiz < 10) && (distanciafr < 10) && (distanciadr < 10)) {
      stop();
      delay(300);
      giro180();
      stop();
      delay(200);
    }
    else{
      stop();
      delay(300);
      atras();
      delay(500);
  }
}

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
}