I'm using the DMP example from Jeff Rowberg's MPU library (with Arduino UNO) and it works well. But with this simple modification it works for a while and then stops. I don't understand why this is happening. If someone knows this will help me a lot. Thanks!
//GND - GND
//VCC - VCC
//SDA - Pin A4
//SCL - Pin A5
//INT - Pin 2
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu; //Declaramos el sensor en la dirección por defecto (0x68),AD0 a LOW
#define INTERRUPT_PIN 2 //Definir el pin de interrupciones
#define LED_PIN 13
bool blinkState = false;
//Variables de control y estado del MPU
bool dmpReady = false; // Es true si la inicialización del DMO es exitosa
uint8_t mpuIntStatus; // Mantiene el estado actual del byte de las interrupciones
uint8_t devStatus; // Devuelve el estado después de cada operación del dispositivo(0 = exitoso, !0 = error)
uint16_t packetSize; // Tamaño esprado del paquete (por defecto es 42 bytes)
uint16_t fifoCount; // Cuenta todos los valores actules en el FIFO
uint8_t fifoBuffer[64]; // Buffer de almacenamiento
//Variables de orientación y movimiento
Quaternion q; // [w, x, y, z]
VectorInt16 aa; // [x, y, z]
VectorInt16 aaReal; // [x, y, z]
VectorInt16 aaWorld; // [x, y, z]
VectorFloat gravity; // [x, y, z]
float ypr[3]; // [yaw, pitch, roll]
float yaw;
float pitch;
float roll;
int izquierda = 0;
int derecha = 0;
volatile bool mpuInterrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
//Iniciar comunicación con el bus I2C
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
// Iniciar MPU6050
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// Comprobar conexion
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// Iniciar DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// Valores de calibracion
mpu.setXGyroOffset(219);
mpu.setYGyroOffset(75);
mpu.setZGyroOffset(-19); //85
mpu.setZAccelOffset(1688);
// Activar DMP
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// Activar interrupcion
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop() {
// Si fallo al iniciar, parar programa
if (!dmpReady) return;
// Ejecutar mientras no hay interrupcion
while (!mpuInterrupt && fifoCount < packetSize) {
// AQUI EL RESTO DEL CODIGO DE TU PROGRRAMA
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// Obtener datos del FIFO
fifoCount = mpu.getFIFOCount();
// Controlar overflow
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.resetFIFO();
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// Mostrar Yaw, Pitch, Roll
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
yaw = -ypr[0] * 180 / M_PI;
pitch = ypr[1] * 180 / M_PI;
roll = ypr[2] * 180 / M_PI;
if (yaw > 90) {
izquierda = 1;
derecha = 0;
}
if (roll > 90) {
derecha = 1;
izquierda = 0;
}
if (yaw < 90 && roll < 90) {
derecha = 0;
izquierda = 0;
}
Serial.print("ángulos\t");
Serial.print(yaw);
Serial.print("\t");
Serial.print(pitch);
Serial.print("\t");
Serial.print(roll);
Serial.print("\t");
Serial.print("izquierda");
Serial.print("\t");
Serial.print(izquierda);
Serial.print("\t");
Serial.print("derecha");
Serial.print("\t");
Serial.println(derecha);
}
}