Librerías de comunicación I2c desarrolladas por Jeff Rowberg:

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

Código base obtenido de:

https://howtomechatronics.com/projects/diy-arduino-gimbal-self-stabilizing-platform/

Vídeo explicativo

Autores

Álvaro Díaz Fernández

Enrique Fernández Godoy

Ángel González Rodríguez

Código completo

/*
  Código basado en el ejemplo MPU6050_DMP6 de la biblioteca i2cdevlib de Jeff Rowberg:
  https://github.com/jrowberg/i2cdevlib
*/
// I2Cdev y MPU6050 deben estar instalados como bibliotecas, o los archivos .cpp/.h
// para ambas clases deben estar en la ruta de inclusión de tu proyecto
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

// La biblioteca Wire de Arduino es requerida si se usa la implementación I2CDEV_ARDUINO_WIRE
// en I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include <Servo.h>

MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- usar para AD0 alto

// Declaración de los 3 servos.
Servo servoZ;
Servo servoY;
Servo servoX;
float correcto;
int j = 0;  //Contador para la calibración al inicio.

#define UMBRAL 100  // Zona sin movimiento del joystick, para asegurar que no se mueva 
//si el jostick no se mueve.

#define SALIDA_LEIBLE_YAWPITCHROLL

#define PIN_INTERRUPCION 2  // usar el pin 2 en Arduino Uno y la mayoría de las placas

bool estadoParpadeo = false;

// Variables de control/estado de MPU
bool dmpListo = false;  // establecer en verdadero si la inicialización de DMP fue exitosa
uint8_t estadoIntMpu;   // contiene el byte de estado de interrupción actual del MPU
uint8_t estadoDev;      // estado de retorno después de cada operación del dispositivo (0 = éxito, !0 = error)
uint16_t tamanioPaquete;// tamaño esperado del paquete DMP (el valor por defecto es 42 bytes)
uint16_t contadorFifo;  // conteo de todos los bytes actualmente en FIFO
uint8_t bufferFifo[64]; // buffer de almacenamiento FIFO

// Variables de orientación/movimiento
Quaternion q;           // [w, x, y, z]         contenedor de cuaterniones
VectorInt16 aa;         // [x, y, z]            mediciones del sensor de aceleración
VectorInt16 aaReal;     // [x, y, z]            mediciones del sensor de aceleración sin gravedad
VectorInt16 aaMundo;    // [x, y, z]            mediciones del sensor de aceleración en el marco del mundo
VectorFloat gravedad;   // [x, y, z]            vector de gravedad
float euler[3];         // [psi, theta, phi]    contenedor de ángulos de Euler
float ypr[3];           // [yaw, pitch, roll]   contenedor de yaw/pitch/roll y vector de gravedad
float anguloZ = 0; // Ángulo girado alrededor del eje Z
unsigned long tiempoAnterior = 0; // Tiempo anterior para cálculo de delta

// Variables para el joystick
const int PIN_JOYSTICK_X = A0; // Pin  eje X del joystick
const int PIN_JOYSTICK_Y = A1; // Pin  eje Y del joystick
const int NEUTRAL_X = 830; // Valor neutro de eje X
const int NEUTRAL_Y = 815; // Valor neutro del eje Y
const int PASO_OFFSET = 1; // Paso  para el servo
int offsetServoZ = 0; // Offset servo Z
int offsetServoX = 0; // Offset servo X



// ================================================================
// ===               RUTINA DE DETECCIÓN DE INTERRUPCIONES       ===
// ================================================================

volatile bool interrupcionMpu = false;     // Indica si se produce una interrupcion por parte del mpu.
void dmpDatosListos()
{
  interrupcionMpu = true;
}

// ================================================================
// ===                      CONFIGURACIÓN INICIAL                ===
// ================================================================

void setup() {
  // unirse al bus I2C (la biblioteca I2Cdev no hace esto automáticamente)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz reloj I2C. Comenta esta línea si tienes dificultades de compilación
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // inicializar la comunicación serie
  Serial.begin(38400);
  while (!Serial); // esperar a la enumeración de Leonardo, otros continúan inmediatamente

  // inicializar el dispositivo
  Serial.println(F("Inicializando dispositivos I2C..."));
  mpu.initialize();
  pinMode(PIN_INTERRUPCION, INPUT);
  estadoDev = mpu.dmpInitialize();

  // proporciona tus propios offsets de giroscopio aquí, escalados para la sensibilidad mínima
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551); 

  // asegúrate de que funcionó (devuelve 0 si es así)
  if (estadoDev == 0) {
    // activar el DMP, ahora que está listo
    Serial.println(F("Activando DMP..."));
    mpu.setDMPEnabled(true);

    attachInterrupt(digitalPinToInterrupt(PIN_INTERRUPCION), dmpDatosListos, RISING);
    estadoIntMpu = mpu.getIntStatus();

    // establecer nuestra bandera DMP Listo para que la función principal loop() sepa que está bien usarlo
    Serial.println(F("DMP listo! Esperando la primera interrupción..."));
    dmpListo = true;

    // obtener el tamaño esperado del paquete DMP para comparación posterior
    tamanioPaquete = mpu.dmpGetFIFOPacketSize();
  } else {
    // ¡ERROR!
    // 1 = carga de memoria inicial fallida
    // 2 = actualizaciones de configuración de DMP fallidas
    // (si va a fallar, generalmente el código será 1)
    Serial.print(F("La inicialización de DMP falló (código "));
    Serial.print(estadoDev);
    Serial.println(F(")"));
  }

  // Conexión de los servos.
  servoZ.attach(11);
  servoY.attach(10);
  servoX.attach(9);
}
// ================================================================
// ===                    BUCLE PRINCIPAL DEL PROGRAMA           ===
// ================================================================

void loop() {
  // si la programación falló, no intentes hacer nada
  if (!dmpListo) return;

  // esperar a la interrupción del MPU o a paquetes adicionales disponibles
  while (!interrupcionMpu && contadorFifo < tamanioPaquete) {
    if (interrupcionMpu && contadorFifo < tamanioPaquete) {
      // intentar salir del bucle infinito
      contadorFifo = mpu.getFIFOCount();
    }
  }

  // restablecer la bandera de interrupción y obtener el byte INT_STATUS
  interrupcionMpu = false;
  estadoIntMpu = mpu.getIntStatus();

  // obtener el conteo actual de FIFO
  contadorFifo = mpu.getFIFOCount();

  // verificar desbordamiento (esto nunca debería suceder a menos que nuestro código sea demasiado ineficiente)
  if ((estadoIntMpu & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || contadorFifo >= 1024) {
    // restablecer para que podamos continuar limpiamente
    mpu.resetFIFO();
    contadorFifo = mpu.getFIFOCount();
    Serial.println(F("¡Desbordamiento de FIFO!"));

    // de lo contrario, verificar la interrupción de datos DMP listos (esto debería suceder con frecuencia)
  } else if (estadoIntMpu & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
    // esperar a la longitud de datos disponibles correcta, debería ser una espera MUY corta
    while (contadorFifo < tamanioPaquete) contadorFifo = mpu.getFIFOCount();

    // leer un paquete de FIFO
    mpu.getFIFOBytes(bufferFifo, tamanioPaquete);

    // rastrear el conteo de FIFO aquí en caso de que haya más de 1 paquete disponible
    // (esto nos permite leer más inmediatamente sin esperar una interrupción)
    contadorFifo -= tamanioPaquete;

    // Obtener valores de Yaw, Pitch y Roll
#ifdef SALIDA_LEIBLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, bufferFifo);
    mpu.dmpGetGravity(&gravedad, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravedad);

    // Valores de Yaw, Pitch, Roll - Radianes a grados
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Saltar 300 lecturas (proceso de auto-calibración)
    if (j <= 300) {
      correcto = ypr[0]; // Yaw comienza en un valor aleatorio, así que capturamos el último valor después de 300 lecturas
      j++;
    }
    // Después de 300 lecturas
    else {
      ypr[0] = ypr[0] - correcto; // Establecer el Yaw en 0 grados - restar el último valor aleatorio de Yaw del valor actual para hacer que el Yaw sea 0 grados

      // Obtener la velocidad angular en el eje Z
        int16_t gx, gy, gz;
        mpu.getRotation(&gx, &gy, &gz);

        // Calcular el ángulo de giro alrededor del eje Z
        unsigned long tiempoActual = millis();
        float deltaTiempo = (tiempoActual - tiempoAnterior) / 1000.0; // Convertir a segundos
        anguloZ += (gz * deltaTiempo) * 0.05; // Integrar la velocidad angular para obtener el ángulo
        tiempoAnterior = tiempoActual;

        // Leer los valores del joystick
        int joystickX = analogRead(PIN_JOYSTICK_X); // Leer el eje X
        int joystickY = analogRead(PIN_JOYSTICK_Y); // Leer el eje Y

        // Controlar el servo Z con el joystick en el eje X
        if (joystickX < NEUTRAL_X - UMBRAL) {
            offsetServoZ -= PASO_OFFSET; // Incrementar el offset
        } else if (joystickX > NEUTRAL_X + UMBRAL) {
            offsetServoZ += PASO_OFFSET; // Decrementar el offset
        }

        // Limitar el offset del servo Z
        offsetServoZ = constrain(offsetServoZ, -90, 90); // Limitar el rango del offset

        // Controlar el servo 1 con el joystick en el eje Y
        if (joystickY < NEUTRAL_Y - UMBRAL) {
            offsetServoX += PASO_OFFSET; // Incrementar el offset
        } else if (joystickY > NEUTRAL_Y + UMBRAL) {
            offsetServoX -= PASO_OFFSET; // Decrementar el offset
        }

        // Limitar el offset del servo Y
        offsetServoX = constrain(offsetServoX, -90, 90); // Limitar el rango del offset

      // Mapear los valores del sensor MPU6050 de -90 a 90 a valores adecuados para el control del servo de 0 a 180
      int valorServoZ = map(anguloZ + offsetServoZ, -90, 90, 180, 0);
      int valorServoY = map(ypr[1], -90, 90, 0, 180);
      int valorServoX = map(ypr[2] + offsetServoX, -90, 90, 0, 180);
      
      // Controlar los servos según la orientación del MPU6050
      servoZ.write(valorServoZ);
      servoY.write(valorServoY);
      servoX.write(valorServoX);
    }
#endif
  }
}