Nuestro Código / Our code

 //Inclusión de la librería "Bluepad32" para el procesamiento de los datos de un mando

#include <Bluepad32.h>

/*Declaración de variables utilizadas en el cálculo de las áreas requeridas en los cuatro 

sectores del plano cartesiano, utilizado para obtener ocho tipos de movimientos con un solo Joystick*/

float xS1;

float xS2;

float xS3;

float xS4;

float yS1;

float yS2;

float yS3;

float yS4;

bool xPos = false;

bool xNeg = false;

bool yPos = false;

bool yNeg = false;

bool pass1 = false;

bool pass2 = false;

//Uso del comando #define para asignar un valor a cada objeto LED según la electrónica

#define LED1_PIN 14

#define LED2_PIN 12

#define LED3_PIN 13

#define LED4_PIN 23

#define LED5_PIN 22

#define LED6_PIN 21

//Uso del comando #define para asignar un valor a cada objeto relacionado con el Puente H según la electrónica

#define STBY 32

#define AIN1 33

#define AIN2 25

#define BIN1 26

#define BIN2 27

ControllerPtr myControllers[BP32_MAX_GAMEPADS];// Creación del objeto a usar para procesar los datos del control y asignación de un máximo de controles conectados posibles

//Función a ejecutar para validar la conexión con un mando

void onConnectedController(ControllerPtr ctl) {

bool foundEmptySlot = false;

for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {

if (myControllers[i] == nullptr) {

Serial.printf("Consola: Controlador conectado, index=%d\n", i); // Impresión del número de control conectado en monitor serial para validar la conexión exitosa

ControllerProperties properties = ctl->getProperties(); // Obtención de los datos del control para validar su funcionamiento con controles originales 

Serial.printf("Controller model: %s, VID=0x%04x, PID=0x%04x\n", ctl->getModelName().c_str(), properties.vendor_id,

properties.product_id);

myControllers[i] = ctl;

foundEmptySlot = true; // Asignación de un valor verdadero en caso de validar la conexión correctamente sin alcanzar un número máximo de mandos conectados

break;

}

}

if (!foundEmptySlot) { // Esto se ejecuta si se llega al máximo de controles conectados

Serial.println("Máximo de controles conectados alcanzado");

}

}

//Función a ejecutar en caso de perder la conexión con el control o no establecerla correctamente

void onDisconnectedController(ControllerPtr ctl) {

bool foundController = false;

for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {

if (myControllers[i] == ctl) {

Serial.printf("Consola: Control desconectado, index=%d\n", i); // Impresión del número de control conectado en monitor serial para validar que no existe conexion con el control

myControllers[i] = nullptr;

foundController = true;

break;

}

}

if (!foundController) { // Esto se si no se recibe datos de indice del control y si no exite una conexión exitosa

Serial.println("No se pudo establecer la conexion");

}

}

void dumpGamepad(ControllerPtr ctl) { //Esta funcion nos permite leer los datos recibidos por el control y mostrarlos en el monitor serial para trabajar con ellos

    Serial.printf(

        "idx=%d, dpad: 0x%02x, buttons: 0x%04x, axis L: %4d, %4d, axis R: %4d, %4d, brake: %4d, throttle: %4d, "

        "misc: 0x%02x, gyro x:%6d y:%6d z:%6d, accel x:%6d y:%6d z:%6d\n",

        ctl->index(),        // Controller Index

        ctl->dpad(),         // D-pad

        ctl->buttons(),      // bitmask of pressed buttons

        ctl->axisX(),        // (-511 - 512) left X Axis

        ctl->axisY(),        // (-511 - 512) left Y axis

        ctl->axisRX(),       // (-511 - 512) right X axis

        ctl->axisRY(),       // (-511 - 512) right Y axis

        ctl->brake(),        // (0 - 1023): brake button

        ctl->throttle(),     // (0 - 1023): throttle (AKA gas) button

        ctl->miscButtons(),  // bitmask of pressed "misc" buttons

        ctl->gyroX(),        // Gyro X

        ctl->gyroY(),        // Gyro Y

        ctl->gyroZ(),        // Gyro Z

        ctl->accelX(),       // Accelerometer X

        ctl->accelY(),       // Accelerometer Y

        ctl->accelZ()        // Accelerometer Z

    );

}

/*iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii*/

void processGamepad(ControllerPtr ctl) {

  xS1 = 0.25*ctl->axisY() - ctl->axisY()/ctl->axisY()*100 + 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada X sector 1

  xS2 = -0.25*ctl->axisY() + ctl->axisY()/ctl->axisY()*100 - 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada X sector 2

  xS3 = 0.25*ctl->axisY() + ctl->axisY()/ctl->axisY()*100 - 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada X sector 3

  xS4 = -0.25*ctl->axisY() - ctl->axisY()/ctl->axisY()*100 + 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada X sector 4

  yS1 = 0.25*ctl->axisX() - ctl->axisX()/ctl->axisX()*100 + 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada Y sector 1

  yS2 = -0.25*ctl->axisX() + ctl->axisX()/ctl->axisX()*100 - 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada Y sector 2

  yS3 = 0.25*ctl->axisX() + ctl->axisX()/ctl->axisX()*100 - 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada Y sector 3

  yS4 = -0.25*ctl->axisX() - ctl->axisX()/ctl->axisX()*100 + 25; //Ecuación para trazar los valores aceptables del Joystick en la cordenada Y sector 4

  if(ctl->buttons() == 0x0010){ pass1 = true; pass2 = false;} //Cambio de estado de variables al pulsar botones distintos para un cambio de iluminación

  if(ctl->buttons() == 0x0020){ pass1 = false; pass2 = true;} //Cambio de estado de variables al pulsar botones distintos para un cambio de iluminación

  if(ctl->buttons() == 0x0008){ escape(); } //Uso de función autónoma "escape" para salir de un una situación difícil de manejar

  if(ctl->buttons() == 0x0002){ kick(); } //Uso de función autónoma "kick" para salir realizar un tiro automático

  if(ctl->axisY() <= -100 && ctl->axisY() >= -512){ yPos = true;} else { yPos = false; } //Asignación de variables de estado de la posición del Joystick para el eje positivo de Y

  if(ctl->axisY() >= 100 && ctl->axisY() <= 511){ yNeg = true;} else { yNeg = false; } //Asignación de variables de estado de la posición del Joystick para el eje negativo de Y

  if(ctl->axisX() <= -100 && ctl->axisX() >= -512){ xNeg = true;} else { xNeg = false; } //Asignación de variables de estado de la posición del Joystick para el eje negativo de X

  if(ctl->axisX() >= 100 && ctl->axisX() <= 511){ xPos = true;} else { xPos = false; } //Asignación de variables de estado de la posición del Joystick para el eje positivo de X

  //Codicionantes para detectar la posición del joystick y ejecutar acciones de movimiento y visualización del movimiento según las comparativas pertinentes para todos los sectores del mapa cartesiano

  //Movimientos Principales: Adelante, Atrás, Izquierda y Derecha

  if(ctl->buttons() == 0x0001){ hstop(); digitalWrite(LED5_PIN, HIGH); digitalWrite(LED6_PIN, HIGH); }

  else if (ctl->axisY() >= -100 && ctl->axisY() <=100 && ctl->axisX() >= -100 && ctl->axisX() <= 100){ stop(); digitalWrite(LED2_PIN, LOW); digitalWrite(LED3_PIN, LOW); digitalWrite(LED5_PIN, LOW); digitalWrite(LED6_PIN, LOW); }

  if(yNeg == true && ctl->axisX() >= xS1 && ctl->axisX() <= xS2){ backward(); digitalWrite(LED5_PIN, HIGH); digitalWrite(LED6_PIN, HIGH);}

  if(yPos == true && ctl->axisX() >= xS3 && ctl->axisX() <= xS4){ forward(); digitalWrite(LED2_PIN, HIGH); digitalWrite(LED3_PIN, HIGH);} 

  if(xNeg == true && ctl->axisY() >= yS1 && ctl->axisY() <= yS2){ rollLeft(); }

  if(xPos == true && ctl->axisY() >= yS4 && ctl->axisY() <= yS3){ rollRight(); }

  //Movimientos Secundarios: Giros sobre una sola rueda en las cuatro direcciones posibles

  if(ctl->axisX() <= -203 && ctl->axisX() >=-512 && ctl->axisY() <= -203 && ctl->axisX() >= -512){ turnLeft(); digitalWrite(LED2_PIN, HIGH); digitalWrite(LED3_PIN, HIGH); }

  if(ctl->axisX() >= 203 && ctl->axisX() <=511 && ctl->axisY() <= -203 && ctl->axisX() >= -512){ turnRight(); digitalWrite(LED2_PIN, HIGH); digitalWrite(LED3_PIN, HIGH); }

  if(ctl->axisX() >= 203 && ctl->axisX() <=511 && ctl->axisY() >= 203 && ctl->axisX() <= 511){ turnbLeft(); digitalWrite(LED5_PIN, HIGH); digitalWrite(LED6_PIN, HIGH); }

  if(ctl->axisX() <= -203 && ctl->axisX() >=-512 && ctl->axisY() >= 203 && ctl->axisX() <= 511){ turnbRight(); digitalWrite(LED5_PIN, HIGH); digitalWrite(LED6_PIN, HIGH); }

  dumpGamepad(ctl);

}

/*iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii*/

void processControllers() { //Función para iniciar el recibimiento de datos

  /*Verificación de conexión exitosa con el control para recibir datos y ejecutar la función "processGamepad()" 

  encargada de obtener valores de botones pulsados y enviar ordenes a motores y LEDs*/

  for (auto myController : myControllers) {

  if (myController && myController->isConnected() && myController->hasData() && myController->isGamepad()) {

    processGamepad(myController); }

}

}

//Conjunto de funciones para la movilidad controlada del robot:

void forward(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, LOW); digitalWrite(AIN2, HIGH); digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); } //1

void backward(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); } //6

void turnLeft(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, LOW); digitalWrite(AIN2, HIGH); digitalWrite(BIN1, LOW); digitalWrite(BIN2, LOW); } //2

void turnRight(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, LOW); digitalWrite(AIN2, LOW); digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); } //3

void rollLeft(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, LOW); digitalWrite(AIN2, HIGH); digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); } //8

void rollRight(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); } //7

void turnbLeft(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, LOW); digitalWrite(AIN2, LOW); digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); } //4

void turnbRight(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); digitalWrite(BIN1, LOW); digitalWrite(BIN2, LOW); } //5

void stop(){ digitalWrite(STBY, LOW); digitalWrite(AIN1, LOW); digitalWrite(AIN2, LOW); digitalWrite(BIN1, LOW); digitalWrite(BIN2, LOW); }

void hstop(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, HIGH); digitalWrite(AIN2, HIGH); digitalWrite(BIN1, HIGH); digitalWrite(BIN2, HIGH); }

void escape(){

  digitalWrite(STBY, HIGH); digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); delay(1000);

  digitalWrite(STBY, HIGH); digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); delay(700); stop(); }

void kick(){ digitalWrite(STBY, HIGH); digitalWrite(AIN1, LOW); digitalWrite(AIN2, HIGH); digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); delay(2000); stop(); }

void setup() {

  Serial.begin(115200); //Inicio de comunicación con monitor serial a una velocidad de 115200 baudios

  Serial.printf("Firmware: %s\n", BP32.firmwareVersion()); //Impresión de la version de Bluepad32

  const uint8_t* addr = BP32.localBdAddress();

  Serial.printf("BD Addr: %2X:%2X:%2X:%2X:%2X:%2X\n", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); //Impresión de la dirección hexa-decimal del bluetooth de la tarjeta

  BP32.setup(&onConnectedController, &onDisconnectedController); //Ejecución de funciones de conexion y desconexión del control

  BP32.forgetBluetoothKeys(); //Comando que prohibe la conexion del robot con más de un control

  BP32.enableVirtualDevice(false); //Comando que evita el uso de emuladores para el control del robot

  //Declaración de pines utilizados para LEDs

  pinMode(LED1_PIN, OUTPUT);

  pinMode(LED2_PIN, OUTPUT);

  pinMode(LED3_PIN, OUTPUT);

  pinMode(LED4_PIN, OUTPUT);

  pinMode(LED5_PIN, OUTPUT);

  pinMode(LED6_PIN, OUTPUT);

  //Declaración de pines utilizados para Puente H (Motores)

  pinMode(STBY, OUTPUT); //Pin standby, funciona para establecer en estado "activo" o "incativo" los motores del Puente H

  pinMode(AIN1, OUTPUT); //Derecha atrás

  pinMode(AIN2, OUTPUT); //Derecha enfrente

  pinMode(BIN1, OUTPUT); //Izquierda atrás

  pinMode(BIN2, OUTPUT); //Izquierda enfrente

  //Declaración de variables utilizadas para secuencia indicadora de encendido

  int time = 500;

  int lapse = 80;

  int jlapse = 350;

  //Secuencia de LEDs de inicio

  for (int i = 0; i < 3; i++){

  digitalWrite(LED1_PIN, HIGH); digitalWrite(LED2_PIN, HIGH); digitalWrite(LED3_PIN, HIGH); digitalWrite(LED4_PIN, HIGH); digitalWrite(LED5_PIN, HIGH); digitalWrite(LED6_PIN, HIGH);

  delay(time);

  digitalWrite(LED1_PIN, LOW); digitalWrite(LED2_PIN, LOW); digitalWrite(LED3_PIN, LOW); digitalWrite(LED4_PIN, LOW); digitalWrite(LED5_PIN, LOW); digitalWrite(LED6_PIN, LOW);

  delay(time);

  } 

  digitalWrite(LED1_PIN, LOW); digitalWrite(LED2_PIN, LOW); digitalWrite(LED3_PIN, LOW); digitalWrite(LED4_PIN, LOW); digitalWrite(LED5_PIN, LOW); digitalWrite(LED6_PIN, LOW);

  for (int i = 0; i < 3; i++){

    digitalWrite(LED1_PIN, HIGH); delay(lapse); digitalWrite(LED1_PIN, LOW); delay(lapse);//  LED1

    digitalWrite(LED2_PIN, HIGH); delay(lapse); digitalWrite(LED2_PIN, LOW); delay(jlapse);//  LED2

    digitalWrite(LED3_PIN, HIGH); delay(lapse); digitalWrite(LED3_PIN, LOW); delay(lapse);//  LED3

    digitalWrite(LED4_PIN, HIGH); delay(lapse); digitalWrite(LED4_PIN, LOW); delay(jlapse);//  LED4

  }

  digitalWrite(LED1_PIN, HIGH); digitalWrite(LED2_PIN, HIGH); digitalWrite(LED3_PIN, HIGH); digitalWrite(LED4_PIN, HIGH); digitalWrite(LED5_PIN, HIGH); digitalWrite(LED6_PIN, HIGH);

  delay(time);

  digitalWrite(LED1_PIN, LOW); digitalWrite(LED2_PIN, LOW); digitalWrite(LED3_PIN, LOW); digitalWrite(LED4_PIN, LOW); digitalWrite(LED5_PIN, LOW); digitalWrite(LED6_PIN, LOW);

  delay(time);

}

void loop() { //Bucle general del programa

  bool dataUpdated = BP32.update(); //La función "BP32.update()" nos permite saber si el control se actualiza constantemente con un valor booleano

  //Validación de actualización de los datos emitidos por el control

  if (dataUpdated){

  processControllers(); 

  //Encendido y apagado de LEDs laterales decorativos con uso de las variables antes mencionadas "pass1" y "pass2"

  if (pass1 == true){digitalWrite(LED1_PIN, HIGH); digitalWrite(LED4_PIN, HIGH);} 

  else if (pass2 == true){digitalWrite(LED1_PIN, LOW); digitalWrite(LED4_PIN, LOW);}

  }

    delay(1);

}