Fix typos

Source Link

edited Nov 4, 2017 at 18:54

4.3k
2
24
44

MPU-6050 +arduino+ Arduino Micro + Nrf24l01nRF24L01 FIFO overflow!

So I have a arduino microan Arduino Micro, nrf24l01nRF24L01, and mpu6050MPU-6050 all attached together and its transmitting information to another nrf24l01nRF24L01 on an arduino unoArduino Uno. The problem is that I keep getting FIFO overflow and on the sending side. If I leave the sensor holding still it goes going for some time, but when I start moving the sensor around it halts, like it can't take that much data when the sensor is moving around. So I think its something with the code. Here's the code for:

// Include libraries necessary for the radios
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

//set up name for serial communication of the radio
RF24 radio(9, 10);

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 * ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * ========================================================================= */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// We are using this output to obtain global coordinate acceleration
//and euler angles (coordinate system irrelevant)
#define OUTPUT_READABLE_WORLDACCEL_AND_GYRO

// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    //Serial.begin(115200);
    //while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // Initialize radio communication
    radio.begin();
    radio.setRetries(15, 15);
    radio.openWritingPipe(0xF0F0F0F0A1LL);
  
    radio.stopListening();

    Serial.begin(115200);

    
    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    /*
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again
    */

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(54);
    mpu.setYGyroOffset(15);
    mpu.setZGyroOffset(33);
    mpu.setZAccelOffset(1613); // 1688 factory default for my test chip
    mpu.setXAccelOffset(-4009);
    mpu.setYAccelOffset(332);

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        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(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);

}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .

    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();



    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        char fifo_text[]="FIFO overflow!";
        radio.write(&fifo_text, sizeof(fifo_text));
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } 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);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL_AND_GYRO
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            //Serial.print("aworld\t");
            //Serial.print(aaWorld.x);
            //Serial.print("\t");
            //Serial.print(aaWorld.y);
            //Serial.print("\t");
            //Serial.print(aaWorld.z);
            //radio.write(&aaWorld.z,sizeof(aaWorld.z));
            //String x=String(aaWorld.x);
            //const char*text=x.c_str();

            /*
            char x_text[]="X acceleration";
            radio.write(&x_text, sizeof(x_text));
            */
            typedef struct Value {
              int x;
              int y;
              int z;
              int e0;
              int e1;
              int e2;   
            } Value;

            Value value;

            value.x=aaWorld.x;
            value.y=aaWorld.y;
            value.z=aaWorld.z;

            /*
            float acceleration[3];
            acceleration[0]=aaWorld.x;
            acceleration[1]=aaWorld.y;
            
            char x_value[8];
            char y_value[8];
            
            dtostrf(aaWorld.x,5,2,x_value);
            dtostrf(aaWorld.y,5,2,y_value);
            //char* coordinates[]={x_value,y_value};
            //char* coordinates[]={x_value,y_value};
            int coordinates[]={aaWorld.x,aaWorld.y};
            //radio.write(&y_value, sizeof(y_value));
            //radio.write(&coordinates, sizeof(coordinates));
            */
            
            
            //Serial.println(aaWorld);
            mpu.dmpGetEuler(euler, &q);
            
            //Serial.print("euler\t");
            int euler_0=euler[0] * 180/M_PI;
            value.e0=euler_0;
            //Serial.print("\t");
            int euler_1=euler[1] * 180/M_PI;
            value.e1=euler_1;
            //Serial.print("\t");
            int euler_2=euler[2] * 180/M_PI;
            value.e2=euler_2;
            //euler=(int[3])euler;
            //int euler[]={1,2,3};
            //Serial.write(euler*180/M_PI,3);
            //delay(50);

            radio.write(&value, sizeof(value));
            Serial.println(aaWorld.z);
            
        #endif
    /*
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif
*/
        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

// Include libraries necessary for the radios
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

//set up name for serial communication of the radio
RF24 radio(9, 10);

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 * ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * ========================================================================= */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// We are using this output to obtain global coordinate acceleration
//and euler angles (coordinate system irrelevant)
#define OUTPUT_READABLE_WORLDACCEL_AND_GYRO

// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    //Serial.begin(115200);
    //while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // Initialize radio communication
    radio.begin();
    radio.setRetries(15, 15);
    radio.openWritingPipe(0xF0F0F0F0A1LL);
  
    radio.stopListening();

    Serial.begin(115200);

    
    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    /*
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again
    */

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(54);
    mpu.setYGyroOffset(15);
    mpu.setZGyroOffset(33);
    mpu.setZAccelOffset(1613); // 1688 factory default for my test chip
    mpu.setXAccelOffset(-4009);
    mpu.setYAccelOffset(332);

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        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(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);

}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .

    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();



    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        char fifo_text[]="FIFO overflow!";
        radio.write(&fifo_text, sizeof(fifo_text));
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } 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);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL_AND_GYRO
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            //Serial.print("aworld\t");
            //Serial.print(aaWorld.x);
            //Serial.print("\t");
            //Serial.print(aaWorld.y);
            //Serial.print("\t");
            //Serial.print(aaWorld.z);
            //radio.write(&aaWorld.z,sizeof(aaWorld.z));
            //String x=String(aaWorld.x);
            //const char*text=x.c_str();

            /*
            char x_text[]="X acceleration";
            radio.write(&x_text, sizeof(x_text));
            */
            typedef struct Value {
              int x;
              int y;
              int z;
              int e0;
              int e1;
              int e2;   
            } Value;

            Value value;

            value.x=aaWorld.x;
            value.y=aaWorld.y;
            value.z=aaWorld.z;

            /*
            float acceleration[3];
            acceleration[0]=aaWorld.x;
            acceleration[1]=aaWorld.y;
            
            char x_value[8];
            char y_value[8];
            
            dtostrf(aaWorld.x,5,2,x_value);
            dtostrf(aaWorld.y,5,2,y_value);
            //char* coordinates[]={x_value,y_value};
            //char* coordinates[]={x_value,y_value};
            int coordinates[]={aaWorld.x,aaWorld.y};
            //radio.write(&y_value, sizeof(y_value));
            //radio.write(&coordinates, sizeof(coordinates));
            */
            
            
            //Serial.println(aaWorld);
            mpu.dmpGetEuler(euler, &q);
            
            //Serial.print("euler\t");
            int euler_0=euler[0] * 180/M_PI;
            value.e0=euler_0;
            //Serial.print("\t");
            int euler_1=euler[1] * 180/M_PI;
            value.e1=euler_1;
            //Serial.print("\t");
            int euler_2=euler[2] * 180/M_PI;
            value.e2=euler_2;
            //euler=(int[3])euler;
            //int euler[]={1,2,3};
            //Serial.write(euler*180/M_PI,3);
            //delay(50);

            radio.write(&value, sizeof(value));
            Serial.println(aaWorld.z);
            
        #endif
    /*
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif
*/
        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

MPU-6050 +arduino Micro + Nrf24l01 FIFO overflow!

So I have a arduino micro, nrf24l01 and mpu6050 all attached together and its transmitting information to another nrf24l01 on an arduino uno. The problem is that I keep getting FIFO overflow and on the sending side. If I leave the sensor holding still it goes going for some time, but when I start moving the sensor around it halts, like it can't take that much data when the sensor moving around. So I think its something with the code. Here's the code for

// Include libraries necessary for the radios
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

//set up name for serial communication of the radio
RF24 radio(9, 10);

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 * ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * ========================================================================= */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// We are using this output to obtain global coordinate acceleration
//and euler angles (coordinate system irrelevant)
#define OUTPUT_READABLE_WORLDACCEL_AND_GYRO

// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    //Serial.begin(115200);
    //while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // Initialize radio communication
    radio.begin();
    radio.setRetries(15, 15);
    radio.openWritingPipe(0xF0F0F0F0A1LL);
  
    radio.stopListening();

    Serial.begin(115200);

    
    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    /*
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again
    */

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(54);
    mpu.setYGyroOffset(15);
    mpu.setZGyroOffset(33);
    mpu.setZAccelOffset(1613); // 1688 factory default for my test chip
    mpu.setXAccelOffset(-4009);
    mpu.setYAccelOffset(332);

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        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(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);

}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .

    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();



    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        char fifo_text[]="FIFO overflow!";
        radio.write(&fifo_text, sizeof(fifo_text));
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } 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);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL_AND_GYRO
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            //Serial.print("aworld\t");
            //Serial.print(aaWorld.x);
            //Serial.print("\t");
            //Serial.print(aaWorld.y);
            //Serial.print("\t");
            //Serial.print(aaWorld.z);
            //radio.write(&aaWorld.z,sizeof(aaWorld.z));
            //String x=String(aaWorld.x);
            //const char*text=x.c_str();

            /*
            char x_text[]="X acceleration";
            radio.write(&x_text, sizeof(x_text));
            */
            typedef struct Value {
              int x;
              int y;
              int z;
              int e0;
              int e1;
              int e2;   
            } Value;

            Value value;

            value.x=aaWorld.x;
            value.y=aaWorld.y;
            value.z=aaWorld.z;

            /*
            float acceleration[3];
            acceleration[0]=aaWorld.x;
            acceleration[1]=aaWorld.y;
            
            char x_value[8];
            char y_value[8];
            
            dtostrf(aaWorld.x,5,2,x_value);
            dtostrf(aaWorld.y,5,2,y_value);
            //char* coordinates[]={x_value,y_value};
            //char* coordinates[]={x_value,y_value};
            int coordinates[]={aaWorld.x,aaWorld.y};
            //radio.write(&y_value, sizeof(y_value));
            //radio.write(&coordinates, sizeof(coordinates));
            */
            
            
            //Serial.println(aaWorld);
            mpu.dmpGetEuler(euler, &q);
            
            //Serial.print("euler\t");
            int euler_0=euler[0] * 180/M_PI;
            value.e0=euler_0;
            //Serial.print("\t");
            int euler_1=euler[1] * 180/M_PI;
            value.e1=euler_1;
            //Serial.print("\t");
            int euler_2=euler[2] * 180/M_PI;
            value.e2=euler_2;
            //euler=(int[3])euler;
            //int euler[]={1,2,3};
            //Serial.write(euler*180/M_PI,3);
            //delay(50);

            radio.write(&value, sizeof(value));
            Serial.println(aaWorld.z);
            
        #endif
    /*
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif
*/
        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

MPU-6050 + Arduino Micro + nRF24L01 FIFO overflow!

So I have an Arduino Micro, nRF24L01, and MPU-6050 all attached together and its transmitting information to another nRF24L01 on an Arduino Uno. The problem is that I keep getting FIFO overflow on the sending side. If I leave the sensor holding still it goes going for some time, but when I start moving the sensor around it halts, like it can't take that much data when the sensor is moving around. So I think its something with the code:

// Include libraries necessary for the radios
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

//set up name for serial communication of the radio
RF24 radio(9, 10);

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 * ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * ========================================================================= */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// We are using this output to obtain global coordinate acceleration
//and euler angles (coordinate system irrelevant)
#define OUTPUT_READABLE_WORLDACCEL_AND_GYRO

// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    //Serial.begin(115200);
    //while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // Initialize radio communication
    radio.begin();
    radio.setRetries(15, 15);
    radio.openWritingPipe(0xF0F0F0F0A1LL);
  
    radio.stopListening();

    Serial.begin(115200);

    
    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    /*
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again
    */

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(54);
    mpu.setYGyroOffset(15);
    mpu.setZGyroOffset(33);
    mpu.setZAccelOffset(1613); // 1688 factory default for my test chip
    mpu.setXAccelOffset(-4009);
    mpu.setYAccelOffset(332);

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        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(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);

}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .

    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();



    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        char fifo_text[]="FIFO overflow!";
        radio.write(&fifo_text, sizeof(fifo_text));
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } 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);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL_AND_GYRO
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            //Serial.print("aworld\t");
            //Serial.print(aaWorld.x);
            //Serial.print("\t");
            //Serial.print(aaWorld.y);
            //Serial.print("\t");
            //Serial.print(aaWorld.z);
            //radio.write(&aaWorld.z,sizeof(aaWorld.z));
            //String x=String(aaWorld.x);
            //const char*text=x.c_str();

            /*
            char x_text[]="X acceleration";
            radio.write(&x_text, sizeof(x_text));
            */
            typedef struct Value {
              int x;
              int y;
              int z;
              int e0;
              int e1;
              int e2;   
            } Value;

            Value value;

            value.x=aaWorld.x;
            value.y=aaWorld.y;
            value.z=aaWorld.z;

            /*
            float acceleration[3];
            acceleration[0]=aaWorld.x;
            acceleration[1]=aaWorld.y;
            
            char x_value[8];
            char y_value[8];
            
            dtostrf(aaWorld.x,5,2,x_value);
            dtostrf(aaWorld.y,5,2,y_value);
            //char* coordinates[]={x_value,y_value};
            //char* coordinates[]={x_value,y_value};
            int coordinates[]={aaWorld.x,aaWorld.y};
            //radio.write(&y_value, sizeof(y_value));
            //radio.write(&coordinates, sizeof(coordinates));
            */
            
            
            //Serial.println(aaWorld);
            mpu.dmpGetEuler(euler, &q);
            
            //Serial.print("euler\t");
            int euler_0=euler[0] * 180/M_PI;
            value.e0=euler_0;
            //Serial.print("\t");
            int euler_1=euler[1] * 180/M_PI;
            value.e1=euler_1;
            //Serial.print("\t");
            int euler_2=euler[2] * 180/M_PI;
            value.e2=euler_2;
            //euler=(int[3])euler;
            //int euler[]={1,2,3};
            //Serial.write(euler*180/M_PI,3);
            //delay(50);

            radio.write(&value, sizeof(value));
            Serial.println(aaWorld.z);
            
        #endif
    /*
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif
*/
        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

added 1676 characters in body

Source Link

edited Nov 23, 2016 at 1:26

Dat Ha

2.9k
6
25
46

// Include libraries necessary for the radios #include <SPI.h> #include <nRF24L01.h> #include <RF24.h>

//set up name for serial communication of the radio RF24 radio(9, 10);

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files // for both classes must be in the include path of your project #include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h" //#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif

// class default I2C address is 0x68 // specific I2C addresses may be passed as a parameter here // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) // AD0 high = 0x69 MPU6050 mpu; //MPU6050 mpu(0x69); // <-- use for AD0 high

/* ========================================================================= NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050's INT pin being connected to the Arduino's external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is digital I/O pin 2.

========================================================================= */

/* ========================================================================= NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error when using Serial.write(buf, len). The Teapot output uses this method. The solution requires a modification to the Arduino USBAPI.h file, which is fortunately simple, but annoying. This will be fixed in the next IDE release. For more info, see these links:

http://arduino.cc/forum/index.php/topic,109987.0.html http://code.google.com/p/arduino/issues/detail?id=958

========================================================================= */

// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual // quaternion components in a [w, x, y, z] format (not best for parsing // on a remote host such as Processing or something though) //#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles // (in degrees) calculated from the quaternions coming from the FIFO. // Note that Euler angles suffer from gimbal lock (for more info, see // http://en.wikipedia.org/wiki/Gimbal_lock) //#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ // pitch/roll angles (in degrees) calculated from the quaternions coming // from the FIFO. Note this also requires gravity vector calculations. // Also note that yaw/pitch/roll angles suffer from gimbal lock (for // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) #define OUTPUT_READABLE_YAWPITCHROLL

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration // components with gravity removed. This acceleration reference frame is // not compensated for orientation, so +X is always +X according to the // sensor, just without the effects of gravity. If you want acceleration // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. //#define OUTPUT_READABLE_REALACCEL

// We are using this output to obtain global coordinate acceleration //and euler angles (coordinate system irrelevant) #define OUTPUT_READABLE_WORLDACCEL_AND_GYRO

// uncomment "OUTPUT_TEAPOT" if you want output that matches the // format used for the InvenSense teapot demo //#define OUTPUT_TEAPOT

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) bool blinkState = false;

// MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; }

// ================================================================ // === INITIAL SETUP === // ================================================================

void setup() { // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR. #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif

// initialize serial communication
// (115200 chosen because itInclude islibraries requirednecessary for Teapot Demo output, butthe it'sradios
// really up to you depending on your#include project)
//Serial<SPI.begin(115200);h>
//while (!Serial); // wait for Leonardo enumeration, others#include continue<nRF24L01.h>
#include immediately<RF24.h>

// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handleset thisup baudname ratefor reliablyserial duecommunication to
//of the baud timing being too misaligned with processor ticks. You must useradio
// 38400 or slower in theseRF24 casesradio(9, or use some kind of external separate
// crystal solution for the UART timer.10);

// InitializeI2Cdev radioand communication
radioMPU6050 must be installed as libraries, or else the .begin();
radiocpp/.setRetries(15,h 15);files
radio// for both classes must be in the include path of your project
#include "I2Cdev.openWritingPipe(0xF0F0F0F0A1LL);h"

radio#include "MPU6050_6Axis_MotionApps20.stopListening();h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

Serial// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.begin(115200);h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

// initialize* device=========================================================================
Serial.println(F("Initializing I2C devices. NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin.")); On the Arduino Uno and Mega 2560, this is
mpu   digital I/O pin 2.initialize();
 * ========================================================================= */

//* verify=========================================================================
 connection  NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.println(Fwrite("Testing devicebuf, connections.len). The Teapot output uses this method."));
Serial   The solution requires a modification to the Arduino USBAPI.println(mpuh file, which
   is fortunately simple, but annoying.testConnection() ?This F("MPU6050will connectionbe successful")fixed :in F("MPU6050the connectionnext failed"));IDE
   release. For more info, see these links:

// wait for ready
/*
Serial.println(F("\nSend any character to begin DMP programming and demohttp: "));
while (Serial.available() && Serial//arduino.read()); cc/forum/ empty buffer
while (!Serialindex.available());              php/topic,109987.0.html
   http:// wait for data
while (Serialcode.available() && Serialgoogle.read()); com/p/arduino/issues/detail?id=958
 empty* buffer========================================================================= again
*/

// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(54);
mpu.setYGyroOffset(15);
mpu.setZGyroOffset(33);
mpu.setZAccelOffset(1613); // 1688 factory default for my test chip
mpu.setXAccelOffset(-4009);
mpu.setYAccelOffset(332);

// make sure it worked (returnsuncomment 0"OUTPUT_READABLE_QUATERNION" if so)
ifyou (devStatuswant ==to 0)see {
the actual
// quaternion components //in turna on[w, thex, DMPy, nowz] thatformat it's(not readybest for parsing
// on a remote Serial.println(F("Enablinghost DMP..."));
such as Processing or mpu.setDMPEnabled(truesomething though);
//#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if //you enablewant Arduinoto interruptsee detectionEuler angles
// (in degrees) calculated Serial.println(F("Enablingfrom interruptthe detectionquaternions (Arduinocoming externalfrom interruptthe 0)..FIFO."));
// Note that Euler attachInterrupt(0,angles dmpDataReady,suffer RISING);
from gimbal lock (for mpuIntStatusmore =info, mpusee
// http://en.getIntStatus(wikipedia.org/wiki/Gimbal_lock);
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// setpitch/roll ourangles DMP(in Readydegrees) flagcalculated sofrom the mainquaternions loop()coming
// functionfrom knowsthe it'sFIFO. okayNote tothis usealso itrequires gravity vector calculations.
// Also note that Serial.println(F("DMPyaw/pitch/roll ready!angles Waitingsuffer forfrom firstgimbal interrupt..."));
lock (for
// more info, dmpReadysee: =http://en.wikipedia.org/wiki/Gimbal_lock)
#define true;OUTPUT_READABLE_YAWPITCHROLL

    // getuncomment expected"OUTPUT_READABLE_REALACCEL" DMPif packetyou sizewant forto latersee comparisonacceleration
  // components packetSizewith =gravity mpuremoved.dmpGetFIFOPacketSize();
} else {
 This acceleration reference //frame ERROR!is
    // 1 = initial memory loadnot failed
compensated for orientation, so //+X 2is =always DMP+X configurationaccording updatesto failedthe
    // (if it's going to breaksensor, usually the code will bejust 1)
without the effects of Serialgravity.print(F("DMP InitializationIf failedyou (codewant "));acceleration
  // compensated Serial.print(devStatus);
for orientation, us OUTPUT_READABLE_WORLDACCEL Serialinstead.println(F(")"));
}//#define OUTPUT_READABLE_REALACCEL

// configureWe LEDare forusing this output to obtain global coordinate acceleration
pinMode//and euler angles (LED_PIN,coordinate OUTPUTsystem irrelevant);

}

// ================================================================ // === MAIN PROGRAM LOOP === // ================================================================

void loop() { // if programming failed, don't try to do anything if (!dmpReady) return;

#define OUTPUT_READABLE_WORLDACCEL_AND_GYRO

// waituncomment for"OUTPUT_TEAPOT" MPUif interruptyou orwant extraoutput packet(s)that availablematches the
while// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define LED_PIN 13 // (!mpuInterruptArduino &&is fifoCount13, <Teensy packetSizeis 11, Teensy++ is 6)
bool {blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // otherset programtrue behaviorif stuffDMP hereinit was successful
uint8_t mpuIntStatus;   // .holds actual interrupt status byte from MPU
uint8_t devStatus;      // .return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // .expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // ifcount youof areall reallybytes paranoidcurrently youin canFIFO
uint8_t frequentlyfifoBuffer[64]; test// inFIFO betweenstorage otherbuffer

// orientation/motion vars
Quaternion q;           // stuff[w, tox, seey, ifz] mpuInterrupt is true      quaternion container
VectorInt16 aa;         // [x, andy, ifz] so           accel sensor measurements
VectorInt16 aaReal;     // [x, "break;"y, fromz] the           gravity-free accel sensor measurements
VectorInt16 aaWorld;    // while()[x, loopy, toz] immediately process the MPU data       world-frame accel sensor measurements
VectorFloat gravity;    // .[x, y, z]            gravity vector
float euler[3];         // .[psi, theta, phi]    Euler angle container
float ypr[3];           // .[yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

 
// get================================================================
// current=== FIFO count                    INITIAL SETUP                       ===
fifoCount// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = mpu12; // 400kHz I2C clock (200kHz if CPU is 8MHz).getFIFOCount Comment this line if having compilation difficulties with TWBR.
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    //Serial.begin(115200);
    //while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we canInitialize continueradio cleanlycommunication
    mpuradio.resetFIFObegin();
    char fifo_text[]="FIFOradio.setRetries(15, overflow!";15);
    radio.write(&fifo_text, sizeofopenWritingPipe(fifo_text)0xF0F0F0F0A1LL);
   
  Serial  radio.println(FstopListening("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
             
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpuSerial.getFIFOCountbegin(115200);

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packetinitialize availabledevice
    // Serial.println(this lets us immediately read more without waiting forF("Initializing anI2C interruptdevices..."));
    fifoCount -= packetSize;mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    #ifdef OUTPUT_READABLE_QUATERNION
        // displaywait quaternionfor valuesready
 in easy matrix form:/*
 w x y z
Serial.println(F("\nSend any character to begin DMP programming and mpu.dmpGetQuaternion(&q,demo: fifoBuffer"));
       while (Serial.printavailable("quat\t");
       && Serial.printread(q.w));
  // empty buffer
    while (!Serial.printavailable("\t"));
         Serial.print(q.x);
        Serial.print("\t");
 // wait for data
    while (Serial.printavailable(q.y);
       && Serial.printread("\t"));
     // empty buffer Serial.println(q.z);again
    #endif*/

    #ifdef OUTPUT_READABLE_EULER
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
     load and configure mpu.dmpGetEuler(euler,the &q);DMP
        Serial.printprintln("euler\t");
        Serial.printF(euler[0] * 180/M_PI);
       "Initializing SerialDMP.print("\t");
        Serial.print(euler[1] * 180/M_PI);
        Serial.print("\t""));
      devStatus = Serialmpu.printlndmpInitialize(euler[2] * 180/M_PI);
    #endif

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
   supply your own gyro offsets mpu.dmpGetQuaternion(&qhere, fifoBuffer);
      scaled for mpu.dmpGetGravity(&gravity,min &q);sensitivity
        mpu.dmpGetYawPitchRollsetXGyroOffset(ypr, &q, &gravity54);
        Serialmpu.printsetYGyroOffset("ypr\t"15);
        Serialmpu.printsetZGyroOffset(ypr[0] * 180/M_PI33);
        Serialmpu.printsetZAccelOffset("\t"1613);
   // 1688 factory default for Serial.print(ypr[1]my *test 180/M_PI);chip
        Serialmpu.printsetXAccelOffset("\t"-4009);
        Serialmpu.printlnsetYAccelOffset(ypr[2] * 180/M_PI332);
    #endif

    #ifdef OUTPUT_READABLE_REALACCEL
        // display real acceleration, adjusted to remove gravity
        mpu.dmpGetQuaternion(&q, fifoBuffer);
    make sure it worked mpu.dmpGetAccel(&aa, fifoBuffer);
      returns 0 mpu.dmpGetGravity(&gravity,if &qso);
       if mpu.dmpGetLinearAccel(&aaReal,devStatus &aa,== &gravity0); {
        Serial.print("areal\t");
// turn on the DMP, now that it's Serial.print(aaReal.x);ready
        Serial.printprintln("\t");
       F("Enabling SerialDMP.print(aaReal.y);
        Serial.print("\t""));
        Serialmpu.printlnsetDMPEnabled(aaReal.ztrue);
    #endif

    #ifdef OUTPUT_READABLE_WORLDACCEL_AND_GYRO
   // enable Arduino interrupt detection
 // display initial world-frame acceleration, adjusted to removeSerial.println(F("Enabling gravityinterrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, anddmpDataReady, rotatedRISING);
 based on known orientation from quaternion  mpuIntStatus = mpu.getIntStatus();

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
        //Serial.print("aworld\t");
        //Serial.print(aaWorld.x);
set our DMP Ready flag so the main //Serial.printloop("\t");
  function knows it's okay to use //Serial.print(aaWorld.y);it
        //Serial.printprintln("\t");
        //Serial.printF(aaWorld.z);
   "DMP ready! Waiting for first //radiointerrupt.write(&aaWorld.z,sizeof(aaWorld.z"));
        //String x=String(aaWorld.x);
       dmpReady //const= char*text=x.c_str();true;

        /*
/ get expected DMP packet size for later charcomparison
 x_text[]="X acceleration";
      packetSize = radiompu.write(&x_text, sizeofdmpGetFIFOPacketSize(x_text));
      } else */{
        typedef// structERROR!
 Value {
      // 1 = initial memory intload x;failed
        // 2 int= y;
DMP configuration updates failed
       int z;
// (if it's going to break, usually the code will intbe e0;1)
         Serial.print(F("DMP intInitialization e1;
failed (code "));
        intSerial.print(devStatus);
 e2;   
     Serial.println(F(")"));
    } Value;

    // configure LED for output
 Value value;  pinMode(LED_PIN, OUTPUT);

        value.x=aaWorld.x;
        value.y=aaWorld.y;
        value.z=aaWorld.z;}

        /*
        float acceleration[3];
        acceleration[0]=aaWorld.x;
        acceleration[1]=aaWorld.y;
        
        char x_value[8];
        char y_value[8];
        
        dtostrf(aaWorld.x,5,2,x_value);
        dtostrf(aaWorld.y,5,2,y_value);
        //char* coordinates[]={x_value,y_value};
        //char* coordinates[]={x_value,y_value};
        int coordinates[]={aaWorld.x,aaWorld.y};
        //radio.write(&y_value, sizeof(y_value));
        //radio.write(&coordinates, sizeof(coordinates));
        */
        
        
        //Serial.println(aaWorld);
        mpu.dmpGetEuler(euler, &q);
        
        //Serial.print("euler\t");
        int euler_0=euler[0] * 180/M_PI;
        value.e0=euler_0;
        //Serial.print("\t");
        int euler_1=euler[1] * 180/M_PI;
        value.e1=euler_1;
        //Serial.print("\t");
        int euler_2=euler[2] * 180/M_PI;
        value.e2=euler_2;
        //euler=(int[3])euler;
        //int euler[]={1,2,3};
        //Serial.write(euler*180/M_PI,3);
        //delay(50);


// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .

    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();



    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        char fifo_text[]="FIFO overflow!";
        radio.write(&value&fifo_text, sizeof(valuefifo_text));
        Serial.println(aaWorldF("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
                 
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.zgetFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif 

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL_AND_GYRO
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            //Serial.print("aworld\t");
            //Serial.print(aaWorld.x);
            //Serial.print("\t");
            //Serial.print(aaWorld.y);
            //Serial.print("\t");
            //Serial.print(aaWorld.z);
            //radio.write(&aaWorld.z,sizeof(aaWorld.z));
            //String x=String(aaWorld.x);
            //const char*text=x.c_str();

            /*
            char x_text[]="X acceleration";
            radio.write(&x_text, sizeof(x_text));
            */
            typedef struct Value {
              int x;
              int y;
              int z;
              int e0;
              int e1;
              int e2;   
            } Value;

            Value value;

            value.x=aaWorld.x;
            value.y=aaWorld.y;
            value.z=aaWorld.z;

            /*
            float acceleration[3];
            acceleration[0]=aaWorld.x;
            acceleration[1]=aaWorld.y;
            
            char x_value[8];
            char y_value[8];
            
            dtostrf(aaWorld.x,5,2,x_value);
            dtostrf(aaWorld.y,5,2,y_value);
            //char* coordinates[]={x_value,y_value};
            //char* coordinates[]={x_value,y_value};
            int coordinates[]={aaWorld.x,aaWorld.y};
            //radio.write(&y_value, sizeof(y_value));
            //radio.write(&coordinates, sizeof(coordinates));
            */
            
            
            //Serial.println(aaWorld);
            mpu.dmpGetEuler(euler, &q);
            
            //Serial.print("euler\t");
            int euler_0=euler[0] * 180/M_PI;
            value.e0=euler_0;
            //Serial.print("\t");
            int euler_1=euler[1] * 180/M_PI;
            value.e1=euler_1;
            //Serial.print("\t");
            int euler_2=euler[2] * 180/M_PI;
            value.e2=euler_2;
            //euler=(int[3])euler;
            //int euler[]={1,2,3};
            //Serial.write(euler*180/M_PI,3);
            //delay(50);

            radio.write(&value, sizeof(value));
            Serial.println(aaWorld.z);
            
        #endif
    /*
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif
*/
        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

*/ // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }