#define m1a 9
#define m1b 2
#define m2a 10
#define m2b 3
#define m3 11
#define fc1 18 //4
#define fc2 19 //5
#define f_iz 0
#define f_der 1
#define f_cen 2
#define t_cen 3

boolean flag_der = false;
boolean flag_izq = false;

void setup() { 

  Serial.begin(57600);
  pinMode(m1a, OUTPUT);
  pinMode(m2a, OUTPUT);  
  pinMode(m1b, OUTPUT); 
  pinMode(m2b, OUTPUT); 
  pinMode(m3, OUTPUT); 
  pinMode(fc1, INPUT); 
  pinMode(fc2, INPUT); 
  digitalWrite(m1a, LOW); 
  digitalWrite(m2a, LOW); 
  digitalWrite(fc1, HIGH); 
  digitalWrite(fc2, HIGH); 
  digitalWrite(m1b, LOW); 
  digitalWrite(m2b, LOW); 

  stop_motores();

}

void loop() {

  if (Serial.available())
  {
    char valor = Serial.read();
    if (valor == 'w') avanza();
    else if (valor == 's') retrocede();
    else if ( (valor == 'a') && (!flag_der) )  izquierda();
    else if ( (valor == 'd') && (!flag_izq) )  derecha();
    else if (valor == 'p') stop_motores();

  }

  Serial.print("fc1 ");
  Serial.print(digitalRead(fc1));
  Serial.print("fc2 ");
  Serial.println(digitalRead(fc2));  


   if( !digitalRead(fc2) && !flag_der )
   {
    flag_der = true;
    motor1(0,0);
   }
  else if (digitalRead(fc2))
    flag_der = false;
    
   if( !digitalRead(fc1) && !flag_izq )
   {
    flag_izq = true;
    motor1(0,0);
   }
  else if (!digitalRead(fc1))
    flag_izq = false;
  

}

void cabeza(int c)
{
  digitalWrite(m3, c);  
}


void stop_motores()
{
  motor2(0,0);
  motor1(0,0);
  cabeza(0);
}

void retrocede ()
{
  motor2(1,0);
}

void avanza ()
{
  motor2(0,1);
}

void derecha()
{
  motor1(1,0);
}

void izquierda()
{
  motor1(0,1);
}

void motor1 (boolean sent, boolean sent1) {
  digitalWrite(m1a, sent); 
  digitalWrite(m1b, sent1);
}

void motor2 (boolean sent, boolean sent1) {
  digitalWrite(m2a, sent); 
  digitalWrite(m2b, sent1);
}