Codigo de apoyo :D

codigo_aceletromo_bt.ino

//Bluetooth
#include "BluetoothSerial.h"
#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;
#define Apagado 0
#define Encendido 1
#define Interminente 2
struct SuperLed {
  String Nombre;
  int Pin;
  int Estado;
  boolean Activo;
  float Tiempo;
};
SuperLed Leds[3] = {
  {"rojo", 5, Apagado, false, 0},
  {"azul", 18, Apagado, false, 0},
  //{"verde", 13, Apagado, false, 0}
};
int CantidadLed = 3;
float IntervaloLed = 200;
//Acelerometro
#include "Wire.h" // This library allows you to communicate with I2C devices.

const int MPU_ADDR = 0x68; // I2C address of the MPU-6050. If AD0 pin is set to HIGH, the I2C address will be 0x69.

int16_t accelerometer_x, accelerometer_y, accelerometer_z; // variables for accelerometer raw data
int16_t gyro_x, gyro_y, gyro_z; // variables for gyro raw data
int16_t temperature; // variables for temperature data

#define LED_LB1 25 // Atras
#define LED_LB2 26 // Atras
#define LED_LB3 27 // Atras
#define LED_LB4 32 // Atras
#define LED_RB1 19  // Adelante
#define LED_RT1 15 // Derecha
#define LED_RT2 2  // Derecha
#define LED_RT3 4  // Derecha
#define LED_LT1 13 // Izquierda
#define LED_LT2 12 // Izquierda
#define LED_LT3 14 // Izquierda
int vel = 100;              // Velocidad de la secuencia
char tmp_str[7]; // temporary variable used in convert function
char* convert_int16_to_str(int16_t i) { // converts int16 to string. Moreover, resulting strings will have the same length in the debug monitor.
  sprintf(tmp_str, "%6d", i);
  return tmp_str;
}
void setup() {
  //BlueTooth
  Serial.begin(115200);
  SerialBT.begin("SR"); //Bluetooth device name
  //Serial.println("The device started, now you can pair it with bluetooth!");
  for (int i = 0; i < CantidadLed; i++) {
    pinMode(Leds[i].Pin, OUTPUT);
  }
  //Acelerometro
  Serial2.begin(9600);
  //Atras
  pinMode(LED_LB1, OUTPUT);
  pinMode(LED_LB2, OUTPUT);
  pinMode(LED_LB3, OUTPUT);
  pinMode(LED_LB4, OUTPUT);
  //Adelante
  pinMode(LED_RB1, OUTPUT);
  //Derecha
  pinMode(LED_RT1, OUTPUT);
  pinMode(LED_RT2, OUTPUT);
  pinMode(LED_RT3, OUTPUT);
  //Izquierda
  pinMode(LED_LT1, OUTPUT);
  pinMode(LED_LT2, OUTPUT);
  pinMode(LED_LT3, OUTPUT);

  //Atras
  digitalWrite(LED_LB1, LOW);
  digitalWrite(LED_LB2, LOW);
  digitalWrite(LED_LB3, LOW);
  digitalWrite(LED_LB4, LOW);
  //Adelante
  digitalWrite(LED_RB1, LOW);
  //Derecha
  digitalWrite(LED_RT1, LOW);
  digitalWrite(LED_RT2, LOW);
  digitalWrite(LED_RT3, LOW);
  //Izquierda
  digitalWrite(LED_LT1, LOW);
  digitalWrite(LED_LT2, LOW);
  digitalWrite(LED_LT3, LOW);

  Wire.begin();
  Wire.beginTransmission(MPU_ADDR); // Begins a transmission to the I2C slave (GY-521 board)
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
}
void loop() {
  //BlueTooth
  if (SerialBT.available()) {
    DecodificarSerial();
  }
  for (int i = 0; i < CantidadLed; i++) {
    ActualizarLed(Leds[i]);
  }
  
  //Actualizar Acelerometro
  ActualizarAcelemetro();
}
void ActualizarLed(SuperLed &Led) {
  switch (Led.Estado) {
    case Apagado:
      digitalWrite(Led.Pin, 0);
      break;
    case Encendido:
      digitalWrite(Led.Pin, 1);
      break;
    case Interminente:
      float TiempoActual = millis();
      if (TiempoActual - Led.Tiempo > IntervaloLed) {
        Led.Tiempo = TiempoActual;
        Led.Activo = !Led.Activo;
        if (Led.Activo) {
          digitalWrite(Led.Pin, 1);
        } else {
          digitalWrite(Led.Pin, 0);
        }
      }
      break;
  }
}
void DecodificarSerial() {
  // rojo/a \n
  // rojo es led
  // a mensaje [a|e|i](Apagado, Encendido, Interminente)
  String Mensaje = SerialBT.readStringUntil('\n');
  Serial.print("Mensaje : ");
  Serial.println(Mensaje);
  int PosicionPleca = Mensaje.indexOf('/');
  int PosicionSaltoLinea = Mensaje.length();
  String Dato = Mensaje.substring(0, PosicionPleca);
  String Valor = Mensaje.substring(PosicionPleca + 1, PosicionSaltoLinea);
  int EstadoActualLed = ObtenerEstadoLed(Valor);
  for (int i = 0; i < CantidadLed; i++) {
    if (Dato.equals(Leds[i].Nombre)) {
      Leds[i].Estado = EstadoActualLed;
      Serial.print("Led : ");
      Serial.print(Dato);
      Serial.print(" Estado : ");
      Serial.println(EstadoActualLed);
      return;
    }
  }
  Serial.println("Error mensaje");
}

int ObtenerEstadoLed(String Valor) {
  if (Valor.equals("a")) {
    return Apagado;
  } else if (Valor.equals("e")) {
    return Encendido;
  } else if (Valor.equals("i")) {
    return Interminente;
  }
  return -1;
}

void ActualizarAcelemetro() {
  //Acelerometro
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(MPU_ADDR, 7 * 2, true); // request a total of 7*2=14 registers

  // "Wire.read()<<8 | Wire.read();" means two registers are read and stored in the same variable
  accelerometer_x = Wire.read() << 8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H) and 0x3C (ACCEL_XOUT_L)
  accelerometer_y = Wire.read() << 8 | Wire.read(); // reading registers: 0x3D (ACCEL_YOUT_H) and 0x3E (ACCEL_YOUT_L)
  accelerometer_z = Wire.read() << 8 | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) and 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read() << 8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H) and 0x42 (TEMP_OUT_L)
  gyro_x = Wire.read() << 8 | Wire.read(); // reading registers: 0x43 (GYRO_XOUT_H) and 0x44 (GYRO_XOUT_L)
  gyro_y = Wire.read() << 8 | Wire.read(); // reading registers: 0x45 (GYRO_YOUT_H) and 0x46 (GYRO_YOUT_L)
  gyro_z = Wire.read() << 8 | Wire.read(); // reading registers: 0x47 (GYRO_ZOUT_H) and 0x48 (GYRO_ZOUT_L)

  // print out data
  Serial2.print("aX = "); Serial2.print(convert_int16_to_str(accelerometer_x));
  Serial2.print(" | aY = "); Serial2.print(convert_int16_to_str(accelerometer_y));
  Serial2.print(" | aZ = "); Serial2.print(convert_int16_to_str(accelerometer_z));
  // the following equation was taken from the documentation [MPU-6000/MPU-6050 Register Map and Description, p.30]
  Serial2.print(" | tmp = "); Serial2.print(temperature / 340.00 + 36.53);
  //Serial.print(" | gX = "); Serial.print(convert_int16_to_str(gyro_x));
  //Serial.print(" | gY = "); Serial.print(convert_int16_to_str(gyro_y));
  //Serial.print(" | gZ = "); Serial.print(convert_int16_to_str(gyro_z));
  Serial2.println();

  if (accelerometer_x < 1000 && accelerometer_y < -4000) {

    //Izquierda//
    digitalWrite(LED_LB1, LOW);
    digitalWrite(LED_LB2, LOW);
    digitalWrite(LED_LB3, LOW);
    digitalWrite(LED_LB4, LOW);

    digitalWrite(LED_RB1, LOW);

    digitalWrite(LED_RT1, LOW);
    digitalWrite(LED_RT2, LOW);
    digitalWrite(LED_RT3, LOW);

    digitalWrite(LED_LT1, HIGH);
    delay(vel);
    digitalWrite(LED_LT2, HIGH);
    delay(vel);
    digitalWrite(LED_LT3, HIGH);
    delay(vel);
    digitalWrite(LED_LT1, LOW);
    delay(vel);
    digitalWrite(LED_LT2, LOW);
    delay(vel);
    digitalWrite(LED_LT3, LOW);
    delay(vel);

  } else if (accelerometer_x < 1000 && accelerometer_y > 4000) {
    //DERECHA//
    digitalWrite(LED_LB1, LOW);
    digitalWrite(LED_LB2, LOW);
    digitalWrite(LED_LB3, LOW);
    digitalWrite(LED_LB4, LOW);

    digitalWrite(LED_RB1, LOW);

    digitalWrite(LED_RT1, HIGH);
    delay(vel);
    digitalWrite(LED_RT2, HIGH);
    delay(vel);
    digitalWrite(LED_RT3, HIGH);
    delay(vel);
    digitalWrite(LED_RT1, LOW);
    delay(vel);
    digitalWrite(LED_RT2, LOW);
    delay(vel);
    digitalWrite(LED_RT3, LOW);
    delay(vel);

    digitalWrite(LED_LT1, LOW);
    digitalWrite(LED_LT2, LOW);
    digitalWrite(LED_LT3, LOW);

    //ADELANTE//
  } else if (accelerometer_x > 4000 && accelerometer_y < 1000) {
    digitalWrite(LED_LB1, LOW);
    digitalWrite(LED_LB2, LOW);
    digitalWrite(LED_LB3, LOW);
    digitalWrite(LED_LB4, LOW);

    digitalWrite(LED_RB1, HIGH);

    digitalWrite(LED_RT1, LOW);
    digitalWrite(LED_RT2, LOW);
    digitalWrite(LED_RT3, LOW);

    digitalWrite(LED_LT1, LOW);
    digitalWrite(LED_LT2, LOW);
    digitalWrite(LED_LT3, LOW);

  } else if (accelerometer_x < -4000 && accelerometer_y < 1000) {

    //ATRAS//
    digitalWrite(LED_LB1, HIGH);
    delay(vel);
    digitalWrite(LED_LB2, HIGH);
    delay(vel);
    digitalWrite(LED_LB3, HIGH);
    delay(vel);
    digitalWrite(LED_LB4, HIGH);
    delay(vel);

    digitalWrite(LED_RB1, LOW);

    digitalWrite(LED_RT1, LOW);
    digitalWrite(LED_RT2, LOW);
    digitalWrite(LED_RT3, LOW);

    digitalWrite(LED_LT1, LOW);
    digitalWrite(LED_LT2, LOW);
    digitalWrite(LED_LT3, LOW);

  } else {
    digitalWrite(LED_LB1, LOW);
    digitalWrite(LED_LB2, LOW);
    digitalWrite(LED_LB3, LOW);
    digitalWrite(LED_LB4, LOW);

    digitalWrite(LED_RB1, LOW);

    digitalWrite(LED_RT1, LOW);
    digitalWrite(LED_RT2, LOW);
    digitalWrite(LED_RT3, LOW);

    digitalWrite(LED_LT1, LOW);
    digitalWrite(LED_LT2, LOW);
    digitalWrite(LED_LT3, LOW);
  }
  // delay
  delay(10);
}