ROBOT OMNIDIRECCIONAL

// 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);
}

// 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);
}