//  4WD Omnidireccional RC
#include <SoftwareSerial.h>
#define RX_PIN A0
#define TX_PIN A1
SoftwareSerial BTSerial(RX_PIN, TX_PIN);
#include <AFMotor.h>
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
int vel = 255;
char valorBoton;
char penultimo = 's'; 
char antepenultimo = 's'; 
char v='s';
void setup()
{
  BTSerial.begin(9600);
  motor1.setSpeed(vel);
  motor2.setSpeed(vel);
  motor3.setSpeed(vel);
  motor4.setSpeed(vel);
}
void loop()
{
  if (BTSerial.available() > 0)
  {
    valorBoton = BTSerial.read();
    
       if(antepenultimo== 'w' && penultimo== 'q' && valorBoton == 'r'){// wqr
        v = 'w';
        adelante();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'w' && penultimo== 'a' && valorBoton == 'f'){// waf 
        v = 'w';
        adelante();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'w' && penultimo== 'd' && valorBoton == 'h'){// wdh
        v = 'w';
        adelante();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'w' && penultimo== 'e' && valorBoton == 'y'){// wey
        v = 'w';
        adelante();
        penultimo = 's';
        antepenultimo== 's';; 
      }
      else if(antepenultimo== 'q' && penultimo== 'w'  && valorBoton == 't'){// qwt
        v = 'q';
        diu();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'q'  && penultimo== 'a' && valorBoton == 'f'){// qaf
        v = 'q';
        diu();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'a' && penultimo=='w' && valorBoton == 't'){//awt
        v = 'a';
        izq();
        penultimo = 's';
        antepenultimo== 's';
      }
       else if(antepenultimo== 'a' && penultimo=='q' && valorBoton == 'r'){//aqr
        v = 'a';
        izq();
        penultimo = 's';
        antepenultimo== 's';
      }
       else if(antepenultimo== 'a' && penultimo=='z' && valorBoton == 'v'){//azv
        v = 'a';
        izq();
        penultimo = 's';
        antepenultimo== 's';
      }
       else if(antepenultimo== 'a' && penultimo=='x' && valorBoton == 'b'){//axb
        v = 'a';
        izq();
        penultimo = 's';
        antepenultimo== 's';
      }
       else if(antepenultimo== 'z' && penultimo=='a' && valorBoton == 'f'){//zaf
        v = 'z';
        did();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'z' && penultimo=='x' && valorBoton == 'b'){//zxb
        v = 'z';
        did();
        penultimo = 's';
        antepenultimo== 's';
      }
       else if(antepenultimo== 'x' && penultimo=='a' && valorBoton == 'f'){//xaf
        v = 'x';
        atras();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'x' && penultimo=='z' && valorBoton == 'v'){//xzv
        v = 'x';
        atras();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'x' && penultimo=='c' && valorBoton == 'n'){//xcn
        v = 'x';
        atras();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'x' && penultimo=='d' && valorBoton == 'h'){//xdh
        v = 'x';
        atras();
        penultimo = 's';
        antepenultimo== 's';
      }
       else if(antepenultimo== 'c' && penultimo=='x' && valorBoton == 'b'){//cxb
        v = 'c';
        ddd();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'c' && penultimo=='d' && valorBoton == 'h'){//cdh
        v = 'c';
        ddd();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'd' && penultimo=='x' && valorBoton == 'b'){//dxb
        v = 'd';
        der();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'd' && penultimo=='c' && valorBoton == 'n'){//dcn
        v = 'd';
        der();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'd' && penultimo=='e' && valorBoton == 'y'){//dey
        v = 'd';
        der();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'd' && penultimo=='w' && valorBoton == 't'){//dwt
        v = 'd';
        der();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'e' && penultimo=='w' && valorBoton == 't') {//ewt
        v = 'e';
        ddu();
        penultimo = 's';
        antepenultimo== 's';
      }
      else if(antepenultimo== 'e' && penultimo=='d' && valorBoton == 'h'){//edh
        v = 'e';
        ddu();
        penultimo = 's';
        antepenultimo== 's';
      }

      
      else if(v == 'w'){//Validacion no se suelta w
        adelante();
        if(valorBoton == 't'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //wq
        gizq();    
        }
        else if(valorBoton == 'a') { //wa
        rizq();    
        }
        else if(valorBoton == 'z') { //wz
        alto();    
        }
        else if(valorBoton == 'x') { //wx
        alto();     
        }
        else if(valorBoton == 'c') { //wc
        alto();     
        }
        else if(valorBoton == 'd') { //wd
        rder();    
        }
        else if(valorBoton == 'e') { //we
        gder();    
        }
      }
      else if(v == 'q'){//Validacion no se suelta  q
        diu();
        if(valorBoton == 'r'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //qw
        gder();    
        }
        else if(valorBoton == 'a') { //qa
        gizq();    
        }
        else if(valorBoton == 'z') { //qz
        alto();    
        }
        else if(valorBoton == 'x') { //qx
        alto();     
        }
        else if(valorBoton == 'c') { //qc
        alto();     
        }
        else if(valorBoton == 'd') { //qd
        alto();    
        }
        else if(valorBoton == 'e') { //qe
        alto();    
        }
      }
      else if(v == 'a'){//Validacion no se suelta  a
        izq();
        if(valorBoton == 'f'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //aq
        gder();    
        }
        else if(valorBoton == 'w') { //aw
        rder();    
        }
        else if(valorBoton == 'z') { //az
        aizq();    
        }
        else if(valorBoton == 'x') { //ax
        ader();     
        }
        else if(valorBoton == 'c') { //ac
        alto();     
        }
        else if(valorBoton == 'd') { //ad
        alto();    
        }
        else if(valorBoton == 'e') { //ae
        alto();    
        }
      }
      else if(v == 'z'){//Validacion no se suelta  z
        did();
        if(valorBoton == 'v'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //zq
        alto();    
        }
        else if(valorBoton == 'a') { //za
        aizq();    
        }
        else if(valorBoton == 'w') { //zw
        alto();    
        }
        else if(valorBoton == 'x') { //zx
        ader();     
        }
        else if(valorBoton == 'c') { //zc
        alto();     
        }
        else if(valorBoton == 'd') { //zd
        alto();    
        }
        else if(valorBoton == 'e') { //ze
        alto();    
        }
      }
      else if(v == 'x'){//Validacion no se suelta  x
        atras();
        if(valorBoton == 'b'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //xq
        alto();    
        }
        else if(valorBoton == 'w') { //xw
        alto();    
        }
        else if(valorBoton == 'z') { //xz
        aizq();    
        }
        else if(valorBoton == 'a') { //xa
        rder();     
        }
        else if(valorBoton == 'c') { //xc
        ader();     
        }
        else if(valorBoton == 'd') { //xd
        rizq();    
        }
        else if(valorBoton == 'e') { //xe
        alto();    
        }
      }
      else if(v == 'c'){//Validacion no se suelta  c
        ddd();
        if(valorBoton == 'n'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //cq
        alto();    
        }
        else if(valorBoton == 'a') { //ca
        alto();    
        }
        else if(valorBoton == 'w') { //cw
        alto();    
        }
        else if(valorBoton == 'x') { //cx
        aizq();     
        }
        else if(valorBoton == 'z') { //cz
        alto();     
        }
        else if(valorBoton == 'd') { //cd
        ader();    
        }
        else if(valorBoton == 'e') { //ce
        alto();    
        }
      }
      else if(v == 'd'){//Validacion no se suelta  d
        der();
        if(valorBoton == 'h'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //dq
        alto();    
        }
        else if(valorBoton == 'a') { //da
        alto();    
        }
        else if(valorBoton == 'w') { //dw
        rizq();    
        }
        else if(valorBoton == 'x') { //dx
        rder();     
        }
        else if(valorBoton == 'z') { //dz
        alto();     
        }
        else if(valorBoton == 'c') { //dc
        ader();    
        }
        else if(valorBoton == 'e') { //de
        gizq();    
        }
      }
      else if(v == 'e'){//Validacion no se suelta  e
        ddu();
        if(valorBoton == 'y'){
         v = 's';
         alto();
         }
        else if(valorBoton == 'q') { //eq
        alto();    
        }
        else if(valorBoton == 'a') { //ea
        alto();    
        }
        else if(valorBoton == 'w') { //ew
        gizq();    
        }
        else if(valorBoton == 'x') { //ex
        alto();     
        }
        else if(valorBoton == 'z') { //ez
        alto();     
        }
        else if(valorBoton == 'd') { //ed
        gder();    
        }
        else if(valorBoton == 'c') { //ec
        alto();    
        }
      }
      
     
      else if(penultimo=='w' && valorBoton == 'q'){// wq
        gizq();
      }
      else if(penultimo=='w' && valorBoton == 'a'){// wa
        rizq();
      }
      else if(penultimo=='w' && valorBoton == 'd'){// wd
        rder();
      }
      else if(penultimo=='w' && valorBoton == 'e'){// we
        gder();
      }
      else if(penultimo=='w' && valorBoton == 'z'){// wz 
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='w' && valorBoton == 'x'){// wx
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='w' && valorBoton == 'c'){// wc
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='q' && valorBoton == 'w'){// qw
        gder();
      }
      else if(penultimo=='q' && valorBoton == 'a'){// qa
        gizq();
      }
      else if(penultimo=='q' && valorBoton == 'e'){// qe
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='q' && valorBoton == 'd'){// qd
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='q' && valorBoton == 'c'){// qc
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='q' && valorBoton == 'x'){//qx
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='q' && valorBoton == 'z'){//qz
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='a' && valorBoton == 'w'){//aw
        rder();
      }
      else if(penultimo=='a' && valorBoton == 'q'){//aq
        gder();
      }
      else if(penultimo=='a' && valorBoton == 'z'){//az
        aizq();
      }
      else if(penultimo=='a' && valorBoton == 'x'){//ax
        rizq();
      }

      else if(penultimo=='a' && valorBoton == 'c'){//ac
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='a' && valorBoton == 'd'){//ad
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='a' && valorBoton == 'e'){//ae
        penultimo = 's';
        antepenultimo== 's';
        alto();
      }     
      else if(penultimo=='z' && valorBoton == 'a'){ //za
        aizq();
      }
      else if(penultimo=='z' && valorBoton == 'x'){ //zx
        ader();
      }
      else if(penultimo=='z' && valorBoton == 'q'){//zq
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='z' && valorBoton == 'w'){//zw
        antepenultimo== 's';
        alto();
      }
      else if(penultimo=='z' && valorBoton == 'e'){//ze
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='z' && valorBoton == 'd'){//zd
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='z' && valorBoton == 'c'){//zc
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }   
      else if(penultimo=='x' && valorBoton == 'a'){ //xa
        rder();
      }
      else if(penultimo=='x' && valorBoton == 'z'){ //xz
        aizq();
      }
      else if(penultimo=='x' && valorBoton == 'c'){ //xc
        ader();
      }
      else if(penultimo=='x' && valorBoton == 'd'){ //xd
        rizq();
      }
      else if(penultimo=='x' && valorBoton == 'q'){//xq
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='x' && valorBoton == 'w'){//xw
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='x' && valorBoton == 'e'){//xe
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }    
      else if(penultimo=='c' && valorBoton == 'x'){ //cx
        aizq();
      }
      else if(penultimo=='c' && valorBoton == 'd'){ //cd
        ader();
      }
      else if(penultimo=='c' && valorBoton == 'z'){//cz
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='c' && valorBoton == 'a'){//ca
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='c' && valorBoton == 'q'){//cq
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='c' && valorBoton == 'w'){//cw
        antepenultimo== 's';
        penultimo = 's';
        alto();
      } 
      else if(penultimo=='c' && valorBoton == 'e'){//ce
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }  
      else if(penultimo=='d' && valorBoton == 'w'){ //dw
        rizq();
      }
      else if(penultimo=='d' && valorBoton == 'e'){ //de
        gizq();
      }
      else if(penultimo=='d' && valorBoton == 'c'){ //dc
        ader();
      }
      else if(penultimo=='d' && valorBoton == 'x'){ //dx
        rder();
      }
      else if(penultimo=='d' && valorBoton == 'q'){//dq
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='d' && valorBoton == 'a'){//da
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='d' && valorBoton == 'z'){//dz
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }     
      else if(penultimo=='e' && valorBoton == 'w'){ //ew
        gizq();
      }
      else if(penultimo=='e' && valorBoton == 'd'){ //ed
        gder();
      }
      else if(penultimo=='e' && valorBoton == 'q') {//eq
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='e' && valorBoton == 'a'){//ea
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='e' && valorBoton == 'z'){//ez
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='e' && valorBoton == 'x'){//ex
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }
      else if(penultimo=='e' && valorBoton == 'c'){//ec
        antepenultimo== 's';
        penultimo = 's';
        alto();
      }


      else if(valorBoton == 'w') { // w
       adelante();
      }
      else if(valorBoton == 'q') { // q
      diu();
      }
      else if(valorBoton == 'a') { // a
      izq();
      }
      else if(valorBoton == 'z') { // z
      did();
      }
      else if(valorBoton == 'x') { // x
      atras();
      }
      else if(valorBoton == 'c') { // c
      ddd();
      }
      else if(valorBoton == 'd') { // d
      der();
      }
      else if(valorBoton == 'e') { // e
      ddu();
      }
        else if(penultimo=='w' && valorBoton == 't'){// wt
        penultimo = 's';
        antepenultimo== 's';
        alto();
        }
        else if(penultimo=='q' && valorBoton == 'r'){// qr
        penultimo = 's';
        antepenultimo== 's';
        alto();
        }
        else if(penultimo=='a' && valorBoton == 'f'){//af
        penultimo = 's';
        antepenultimo== 's';
        alto();
        }
        else if(penultimo=='z' && valorBoton == 'v'){//zv
        antepenultimo== 's';
        penultimo = 's';
        alto();
        }
        else if(penultimo=='x' && valorBoton == 'b'){//xb
        antepenultimo== 's';
        penultimo = 's';
        alto();
        }
        else if(penultimo=='c' && valorBoton == 'n'){//cn
        antepenultimo== 's';
        penultimo = 's';
        alto();
        }  
        else if(penultimo=='d' && valorBoton == 'h'){//dh
        penultimo = 's';
        alto();
        }
        else if(penultimo=='e' && valorBoton == 'y'){//ey
        penultimo = 's';
        alto();
        }      
   antepenultimo = penultimo;   
   penultimo = valorBoton;
  }
}
void adelante() {
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}
void atras()
{
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
}
void izq() //Lateral izquierda
{
  motor1.run(BACKWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(FORWARD);
}
void der()  //Lateral derecha
{
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
  motor3.run(FORWARD);
  motor4.run(BACKWARD);
}
void ddu() { // Diagonal Derecha Superior
  motor1.run(FORWARD);
  motor2.run(RELEASE);
  motor3.run(FORWARD);
  motor4.run(RELEASE);
}
void ddd() { // Diagonal Derecha Inferior
  motor1.run(RELEASE);
  motor2.run(BACKWARD);
  motor3.run(RELEASE);
  motor4.run(BACKWARD);
}
void diu() { //Diagonal Izquierda Superior
  motor1.run(RELEASE);
  motor2.run(FORWARD);
  motor3.run(RELEASE);
  motor4.run(FORWARD);
}

void did() { //Diagonal Izquierda Inferior
  motor1.run(BACKWARD);
  motor2.run(RELEASE);
  motor3.run(BACKWARD);
  motor4.run(RELEASE);
}
void gizq()  //Giro a la izquierda adelante
{
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}
void gder()  //Giro a la derecha adelante
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}
void aizq() { //Giro a la derecha atras
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}
void ader() { //Giro a la derecha atras
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
}
void rder() { //rotacion a la derecha
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}
void rizq() { //rotacion a la izquierda
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
}
void alto() //Paro total
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}