//Evasor de Obstaculos sencillo
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 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);
}

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();
  }
  else if(l1==1 && l2==0 && l3==0)
  {
    derecha();
  }
  else if(l1==0 && l2==1 && l3==1)
  {
    izquierdacerrada();
  }
  else if(l1==0 && l2==0 && l3==1)
  {
    izquierda();
  }
  else if(l1==1 && l2==0 && l3==1)
  {
    adelante();
  }
  ///////////////////////////////////////////////////////////////////
  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,vel);
}

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,vel);
  digitalWrite(motB0,HIGH);
  digitalWrite(motB1,LOW);
}

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