#define IZQTRIGGER 2
#define IZQECHO 3
#define DERTRIGGER 4
#define DERECHO 5
#define ADETRIGGER 6
#define ADEECHO 7

int ENA = 19;
int IN1 = 18;
int IN2 = 17;

int ENB = 14;
int IN3 = 16;
int IN4 = 15;

const float sonido = 34300.0; // Velocidad del sonido en cm/s

void setup ()
{
 Serial.begin(9600);
 pinMode (ENA, OUTPUT);
 pinMode (ENB, OUTPUT);
 pinMode (IN1, OUTPUT);
 pinMode (IN2, OUTPUT);
 pinMode (IN3, OUTPUT);
 pinMode (IN4, OUTPUT);
 pinMode (DERECHO, INPUT);
 pinMode (DERTRIGGER, OUTPUT);
 pinMode (IZQECHO, INPUT);
 pinMode (IZQTRIGGER, OUTPUT);
 pinMode (ADEECHO, INPUT);
 pinMode (ADETRIGGER, OUTPUT);
}


void loop() {
  //delay (5000);
  //while (true){
    izqIniciarTrigger();
    float distanciaizq = calcularDistanciaIzq();
    derIniciarTrigger();
    float distanciader = calcularDistanciaDer();
    adeIniciarTrigger();
    float distanciaade = calcularDistanciaAde();

    if (distanciaade < 10){
      Adelante();
      Serial.println ("RUUUUUUUUUUUUUN");
    } else {
      Parar();
      Serial.println ("bip");
    }

  //}
}


void izqIniciarTrigger()
{
  // Ponemos el Triiger en estado bajo y esperamos 2 ms
  digitalWrite(IZQTRIGGER, LOW);
  //delayMicroseconds(1);

  // Ponemos el pin Trigger a estado alto y esperamos 10 ms
  digitalWrite(IZQTRIGGER, HIGH);
  //delayMicroseconds(2);

  // Comenzamos poniendo el pin Trigger en estado bajo
  digitalWrite(IZQTRIGGER, LOW);
}

void derIniciarTrigger()
{
  digitalWrite(DERTRIGGER, LOW);
  //delayMicroseconds(1);

  digitalWrite(DERTRIGGER, HIGH);
  //delayMicroseconds(2);

  digitalWrite(DERTRIGGER, LOW);
}

void adeIniciarTrigger()
{
  digitalWrite(ADETRIGGER, LOW);
  //delayMicroseconds(1);

  digitalWrite(ADETRIGGER, HIGH);
  //delayMicroseconds(2);

  digitalWrite(ADETRIGGER, LOW);
}



float calcularDistanciaIzq()
{
  unsigned long tiempo = pulseIn(IZQECHO, HIGH);

  float distancia1 = tiempo * 0.000001 * sonido / 2.0;
  Serial.print(distancia1);
  Serial.print("cm");
  Serial.println();

  return distancia1;
}

float calcularDistanciaDer()
{
  unsigned long tiempo = pulseIn(DERECHO, HIGH);

  float distancia2 = tiempo * 0.000001 * sonido / 2.0;
  Serial.print(distancia2);
  Serial.print("cm");
  Serial.println();

  return distancia2;
}

float calcularDistanciaAde()
{
  // La función pulseIn obtiene el tiempo que tarda en cambiar entre estados, en este caso a HIGH
  unsigned long tiempo = pulseIn(ADEECHO, HIGH);

  // Obtenemos la distancia en cm, hay que convertir el tiempo en segudos ya que está en microsegundos
  // por eso se multiplica por 0.000001
  float distancia3 = tiempo * 0.000001 * sonido / 2.0;
  Serial.print(distancia3);
  Serial.print("cm ade");
  Serial.println();

  return distancia3;
}



void Adelante ()
{
 //Direccion motor A
 digitalWrite (IN1, HIGH);
 digitalWrite (IN2, LOW);
 analogWrite (ENA, 255); //Velocidad motor A
 //Direccion motor B
 digitalWrite (IN3, HIGH);
 digitalWrite (IN4, LOW);
 analogWrite (ENB, 255); //Velocidad motor B
}

void Atras ()
{
 //Direccion motor A
 digitalWrite (IN1, LOW);
 digitalWrite (IN2, HIGH);
 analogWrite (ENA, 128); //Velocidad motor A
 //Direccion motor B
 digitalWrite (IN3, LOW);
 digitalWrite (IN4, HIGH);
 analogWrite (ENB, 128); //Velocidad motor B
}

void Derecha ()
{
 //Direccion motor A
 digitalWrite (IN1, HIGH);
 digitalWrite (IN2, LOW);
 analogWrite (ENA, 200); //Velocidad motor A
 //Direccion motor B
 digitalWrite (IN3, LOW);
 digitalWrite (IN4, HIGH);
 analogWrite (ENB, 100); //Velocidad motor A
}

void Izquierda ()
{
 //Direccion motor A
 digitalWrite (IN1, LOW);
 digitalWrite (IN2, HIGH);
 analogWrite (ENA, 50); //Velocidad motor A
 //Direccion motor B
 digitalWrite (IN3, HIGH);
 digitalWrite (IN4, LOW);
 analogWrite (ENB, 150); //Velocidad motor A
}

void Parar ()
{
 //Direccion motor A
 digitalWrite (IN1, LOW);
 digitalWrite (IN2, LOW);
 analogWrite (ENA, 0); //Velocidad motor A
 //Direccion motor B
 digitalWrite (IN3, LOW);
 digitalWrite (IN4, LOW);
 analogWrite (ENB, 0); //Velocidad motor A
}