#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;

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

void loop() {
l1 = digitalRead(sl);
  l2 = digitalRead(sc);
  l3 = digitalRead(sr);
  if ((l1 == 1 && l2 == 1 && l3 == 1) || (l1 == 0 && l2 == 0 && l3 == 0)) {
    if (!cont) {
      tiempo = millis();  
      cont = true;
    } else {
      if (millis() - tiempo >= 3000) {
        alto(); 
        cont = false;
      }
    }
  } else {
    cont = false; 
  }
  if (l1 == 0 && l2 == 1 && l3 == 0) {
    adelante();
  }
  else if ((l1 == 0 && l2 == 0 && l3 == 1) || (l1 == 0 && l2 == 1 && l3 == 1)) {
    der1();
  }
  else if ((l1 == 1 && l2 == 0 && l3 == 0) || (l1 == 1 && l2 == 1 && l3 == 0)) {
    izq1();
  }
  f1 = l1;
  f2 = l2;
  f3 = l3;
}

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