#define m1 27
#define m2 14
#define m3 12
#define m4 13

int sl = 5;
int sc = 17;
int sr = 16;
int l1 = 1;
int l2 = 1;
int l3 = 1;
int f1 = 0;
int f2 = 0;
int f3 = 0;

int ec = 22;
int tr = 23;

unsigned long tiempo = 0;
bool cont = false;

void setup() {
  pinMode(m1, OUTPUT);
  pinMode(m2, OUTPUT);
  pinMode(m3, OUTPUT);
  pinMode(m4, OUTPUT);
  digitalWrite(m1, 0);
  digitalWrite(m2, 0);
  digitalWrite(m3, 0);
  digitalWrite(m4, 0);

  pinMode(sl, INPUT);
  pinMode(sc, INPUT);
  pinMode(sr, INPUT);
  
  pinMode(ec, INPUT);
  pinMode(tr, OUTPUT);
  digitalWrite(tr, LOW);
}

void loop() {
  
  if(distancia()<=20)
  {
    if(digitalRead(sl)==0 || digitalRead(sc)==0 || digitalRead(sr)==0)
      {
      alto();
      delay(200);
      atras();
      delay(500);
      alto();
      delay(500);
      derecha();
      delay(800);
      alto();
      delay(500);
      adelante();
      }
      else
      {
      adelante();
      }
      }
    else
      {
      if(digitalRead(sl)==0 || digitalRead(sc)==0 || digitalRead(sr)==0)
      {
      alto();
      delay(200);
      atras();
      delay(500);
      alto();
      delay(500);
      derecha();
      delay(800);
      alto();
      delay(500);
      adelante();
      }
      else
      {
      adelante();
      }
      }
}

long distancia()
{
long t;
long d;
digitalWrite(tr, HIGH);
delayMicroseconds(10);
digitalWrite(tr, LOW);
t = pulseIn(ec, HIGH, 15000);
d = (t == 0) ? 400 : t / 59;
delay(10);
return d;
}


void alto(){
  digitalWrite(m1, 0);
  digitalWrite(m2, 0);
  digitalWrite(m3, 0);
  digitalWrite(m4, 0);
}

void atras(){
  digitalWrite(m1, 0);
  digitalWrite(m2, 1);
  digitalWrite(m3, 1);
  digitalWrite(m4, 0);
}

void adelante(){
  digitalWrite(m1, 1);
  digitalWrite(m2, 0);
  digitalWrite(m3, 0);
  digitalWrite(m4, 1);
}

void derecha(){
  digitalWrite(m1, 1);
  digitalWrite(m2, 0);
  digitalWrite(m3, 1);
  digitalWrite(m4, 0);
}

void izquierda(){
  digitalWrite(m1, 0);
  digitalWrite(m2, 1);
  digitalWrite(m3, 0);
  digitalWrite(m4, 1);
}

void izq1(){
  digitalWrite(m1, 0);
  digitalWrite(m2, 0); 
  digitalWrite(m3, 0); 
  digitalWrite(m4, 1);
}

void der1(){
  digitalWrite(m1, 1);
  digitalWrite(m2, 0); 
  digitalWrite(m3, 0); 
  digitalWrite(m4, 0);
}

void atrasizquierda(){
  digitalWrite(m1, 0);
  digitalWrite(m2, 0); 
  digitalWrite(m3, 1); 
  digitalWrite(m4, 0);
}

void atrasderecha(){
  digitalWrite(m1, 0);
  digitalWrite(m2, 1); 
  digitalWrite(m3, 0); 
  digitalWrite(m4, 0);
}