//Seguidor de linea y Evasor de Obstáculos 
int motA0=10; // Pines para control de motores
int motA1=11;
int motB0=9;
int motB1=8;

int sensorleft = 5;
int sensorcenter = 6;
int sensorright = 7;

int echo=2;//Sensor de obstaculos 
int triger=3;

int l1 = 1;
int l2 = 1;
int l3 = 1;

int f1 = 0;
int f2 = 0;
int f3 = 0;
int vel = 100;

void setup() {
 pinMode(motA0, OUTPUT);
 pinMode(motA1, OUTPUT);
 pinMode(motB0, OUTPUT);
 pinMode(motB1, OUTPUT);

 digitalWrite(motA0, LOW);
 digitalWrite(motA1, LOW);
 digitalWrite(motB0, LOW);
 digitalWrite(motB1, LOW);
 
 pinMode(sensorleft, INPUT);
 pinMode(sensorcenter, INPUT);
 pinMode(sensorright, INPUT);

 pinMode(echo,INPUT);
 pinMode(triger,OUTPUT);
}

void loop(){
  //Linea Blanca
    
  l1 = digitalRead(sensorleft);     
  l2 = digitalRead(sensorcenter);
  l3 = digitalRead(sensorright);

  /////////////////
  if (l1 == 1 && l2 == 1 && l3 == 1) //Feedback cuando todos los sensores estan en negro
  {
    l1 = f1;
    l2 = f2;
    l3 = f3;
  }
  /////////////////
  else if(l1==1 && l2==1 && l3==0)
  {
    derechacerrada();
    if(distancia() <= 15){
      izquierdacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(300);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(600);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(300);
      adelante();
      if(l1==0 && l2==0 && l3==0){
        adelante();
        delay(200);
        izquierdacerrada();
      }
    }
  }
  else if(l1==1 && l2==0 && l3==0)
  {
    derecha();
    if(distancia() <= 15){
      izquierdacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(300);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(600);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(300);
      adelante();
      if(l1==0 && l2==0 && l3==0){
        adelante();
        delay(200);
        izquierdacerrada();
      }
    }
  }
  else if(l1==0 && l2==1 && l3==1)
  {
    izquierdacerrada();
    if(distancia() <= 15){
      izquierdacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(300);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(600);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(300);
      adelante();
      if(l1==0 && l2==0 && l3==0){
        adelante();
        delay(200);
        izquierdacerrada();
      }
    }
  }
  else if(l1==0 && l2==0 && l3==1)
  {
    izquierda();
    if(distancia() <= 15){
      izquierdacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(300);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(600);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(300);
      adelante();
      if(l1==0 && l2==0 && l3==0){
        adelante();
        delay(200);
        izquierdacerrada();
      }
    }
  }
  else if(l1==1 && l2==0 && l3==1)
  {
    adelante();
    if(distancia() <= 15){
      izquierdacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(300);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(200);
      adelante();
      delay(600);
      alto();
      delay(200);
      derechacerrada();
      delay(500);
      alto();
      delay(300);
      adelante();
      if(l1==0 && l2==0 && l3==0){
        adelante();
        delay(200);
        izquierdacerrada();
      }
    }
  }
  ///////////////////////////////////////////////////////////////////
  f1 = l1;
  f2 = l2;
  f3 = l3;      //memory variables
  ////////////////////////////////////////////////////////////////////
  
}

void adelante()
{
  digitalWrite(motA0, HIGH);
  digitalWrite(motA1, LOW);
  digitalWrite(motB0, HIGH);
  digitalWrite(motB1, LOW);
}

void atras()
{
  digitalWrite(motA0, LOW);
  digitalWrite(motA1, HIGH);
  digitalWrite(motB0, LOW);
  digitalWrite(motB1, HIGH);
}

void izquierda()  //Giro a la izquierda
{
  digitalWrite(motA0,HIGH); 
  digitalWrite(motA1,LOW);
  digitalWrite(motB0,LOW);
  digitalWrite(motB1,LOW);
}

void izquierdacerrada()
{
  digitalWrite(motA0,HIGH); 
  digitalWrite(motA1,LOW);
  digitalWrite(motB0,LOW);
  digitalWrite(motB1,HIGH);
}

void derecha()  //Giro a la derecha
{
  digitalWrite(motA0,LOW); 
  digitalWrite(motA1,LOW);
  digitalWrite(motB0,HIGH);
  digitalWrite(motB1,LOW);
}

void derechacerrada()//Giro a la derecha cerrada
{
  digitalWrite(motA0,LOW); 
  digitalWrite(motA1,HIGH);
  digitalWrite(motB0,HIGH);
  digitalWrite(motB1,LOW);
}

void alto() //Paro total
{
  digitalWrite(motA0,HIGH);
  digitalWrite(motA1,HIGH);
  digitalWrite(motB0,HIGH);
  digitalWrite(motB1,HIGH);
}

long distancia()  //Medicion de distancia
{
  long t;
  long d;
  digitalWrite(triger, HIGH);
  delayMicroseconds(10);
  digitalWrite(triger, LOW);
  
  t = pulseIn(echo, HIGH);
  d = t/59;
             
  delay(10);
  return d;
}