#include "BluetoothSerial.h"

//Configuración para saber si está disponible el Bluetooth, habilitarlo y ver si presenta algún error

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
BluetoothSerial SerialBT; //Creamos un Serial Bluetooth con cualquier nombre, en este caso "SerialBT"

//MOTORES
int grados = 0, pasos = 0;
int M1= 12, M2= 14, M3= 27, M4= 26; //Motor izquierdo
int M5= 5, M6= 18, M7= 19, M8= 21;   //Motor derecho
int del = 5, cont = 0;

void setup(){
  //Bluetooth
  SerialBT.begin("MADRE MIA WILLY"); //Nombre del dispositivo Bluetooth
  Serial.begin(115200); //Velocidad de la comunicación serial

  //Motores
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);

  pinMode(M5, OUTPUT);
  pinMode(M6, OUTPUT);
  pinMode(M7, OUTPUT);
  pinMode(M8, OUTPUT);
}

void loop(){
  if(SerialBT.available()){            //Comprobamos si existe un dato en el monitor serial
    char comando = SerialBT.read();    //En la variable se almacena lo que llega en SerialBT
    Serial.println(comando);           //Imprimimos el valor almacenado

    switch(comando){                   //Según el valor de "comando" se realizará una acción
      case '1':
        Motor_izquierdo();
        delay(500);
        break;
      case '2':
        Motor_derecho();
        delay(500);
        break;
      case '3':
        Adelante();
        delay(500);
        break;
    }
  }
}
void Motor_izquierdo(){
  grados = 250;
  pasos = 0.0;
  while(grados > pasos){ 
    //Uno
    digitalWrite(M1, LOW);
    digitalWrite(M2, LOW);
    digitalWrite(M3, LOW);
    digitalWrite(M4, HIGH);
    delay(del);

    //Dos
    digitalWrite(M1, LOW);
    digitalWrite(M2, LOW);
    digitalWrite(M3, HIGH);
    digitalWrite(M4, LOW);
    delay(del);

    //Tres
    digitalWrite(M1, LOW);
    digitalWrite(M2, HIGH);
    digitalWrite(M3, LOW);
    digitalWrite(M4, LOW); 
    delay(del);  

    //Cuatro
    digitalWrite(M1, HIGH);
    digitalWrite(M2, LOW);
    digitalWrite(M3, LOW);
    digitalWrite(M4, LOW);
    delay(del);
    pasos = pasos + 1;
  }
}
void Motor_derecho(){
  grados = 250;
  pasos = 0.0;
  while(grados > pasos){ 
    //Uno
    digitalWrite(M5, LOW);
    digitalWrite(M6, LOW);
    digitalWrite(M7, LOW);
    digitalWrite(M8, HIGH);
    delay(del);

    //Dos
    digitalWrite(M5, LOW);
    digitalWrite(M6, LOW);
    digitalWrite(M7, HIGH);
    digitalWrite(M8, LOW);
    delay(del);  

    //Tres
    digitalWrite(M5, LOW);
    digitalWrite(M6, HIGH);
    digitalWrite(M7, LOW);
    digitalWrite(M8, LOW);
    delay(del);

    digitalWrite(M5, HIGH);
    digitalWrite(M6, LOW);
    digitalWrite(M7, LOW);
    digitalWrite(M8, LOW);    
    delay(del);
    pasos = pasos + 1;
  }
}
void Adelante(){
  grados = 250;
  pasos = 0.0;
  while(grados > pasos){
    //Uno
    digitalWrite(M1, LOW);
    digitalWrite(M2, LOW);
    digitalWrite(M3, LOW);
    digitalWrite(M4, HIGH);

    digitalWrite(M5, LOW);
    digitalWrite(M6, LOW);
    digitalWrite(M7, LOW);
    digitalWrite(M8, HIGH);
    delay(del);

    //Dos
    digitalWrite(M1, LOW);
    digitalWrite(M2, LOW);
    digitalWrite(M3, HIGH);
    digitalWrite(M4, LOW);

    digitalWrite(M5, LOW);
    digitalWrite(M6, LOW);
    digitalWrite(M7, HIGH);
    digitalWrite(M8, LOW);
    delay(del);

    //Tres
    digitalWrite(M1, LOW);
    digitalWrite(M2, HIGH);
    digitalWrite(M3, LOW);
    digitalWrite(M4, LOW);

    digitalWrite(M5, LOW);
    digitalWrite(M6, HIGH);
    digitalWrite(M7, LOW);
    digitalWrite(M8, LOW);
    delay(del);

    //Cuatro
    digitalWrite(M1, HIGH);
    digitalWrite(M2, LOW);
    digitalWrite(M3, LOW);
    digitalWrite(M4, LOW);
    
    digitalWrite(M5, HIGH);
    digitalWrite(M6, LOW);
    digitalWrite(M7, LOW);
    digitalWrite(M8, LOW);    
    delay(del);
    pasos = pasos + 1;
  }
}