1

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);
  }
}
dda
  • 1,588
  • 1
  • 12
  • 17
Irene
  • 11
  • 1
  • 1
    What simple modification? And where does it stop? Have you tried to hunt down the problem by using Serial prints? – chrisl Feb 01 '20 at 20:36
  • this is my modification: 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; } – Irene Feb 03 '20 at 22:39
  • I've used the serial monitor to print the yaw, pitch and roll. I think it stops in this line : if (!dmpReady) return; But I don´t know why the dmp doesn't work. – Irene Feb 03 '20 at 22:46

0 Answers0