#include<Servo.h>
Servo motorDer;
Servo motorIzq;
String inString;
int lecturaSensorIzq;
int lecturaSensorDer;
int lfL = 2;
int lfR = 3;
void setup() {
  Serial.begin(19200);
  Serial.flush();
  motorDer.attach(8);
  motorIzq.attach(9);
}

void loop() {
  lecturaSensorIzq = digitalRead(lfL);
  lecturaSensorDer = digitalRead(lfR);

  if (Serial.available() > 0) {
    rd();
  }
  //Controles
  if (inString == "up") {
    up();
  }
  else if (inString == "brake") {
    down();
  }
  else if (inString == "left") {
    TurnL();
  }
  else if (inString == "right") {
    TurnR();
  }
  else if (inString == "su") {
    stp();
  }
  //Gestos
  else if (inString == "d1") {
    dance1();
  }
  else if (inString == "d2") {
    dance2();
  }
  else if (inString == "d3") {
    dance3();
  }
  else if (inString == "d4") {
    dance4();
  }
  else if (inString == "d5") {
    dance5();
  }
}
void rd() {
  char InChar;
  while (Serial.available() > 0) {
    InChar = (char) Serial.read();
    Serial.flush();
    if (InChar == '=')
      inString = "";
    else if (InChar != '+')
      inString += InChar;
  }
}
void TurnR() {
  motorDer.write(90);
  motorIzq.write(180);
}
void TurnL() {
  motorDer.write(0);
  motorIzq.write(90);
}
void up() {
  motorDer.write(0);
  motorIzq.write(180);
}
void down() {
  motorDer.write(180);
  motorIzq.write(0);
}
void stp() {
  motorDer.write(90);
  motorIzq.write(90);
}
void dance1() { //girar
  motorDer.write(0);
  motorIzq.write(0);
  delay(1000);
  motorDer.write(180);
  motorIzq.write(180);

}
void dance2() { //siguelineas
  if (  lecturaSensorIzq == 1 &&  lecturaSensorDer == 0 ) {
    TurnR();
  }
  else if (  lecturaSensorDer == 1 && lecturaSensorIzq == 0 ) {
    TurnL();
  }
  else if ( lecturaSensorDer == 1 && lecturaSensorIzq == 1 ) {
    up();
  }

  else if ( lecturaSensorDer == 0 && lecturaSensorIzq == 0 ) {
    stp();
  }

}
void dance3() {


}
void dance4() {


}
void dance5() {


}