Probably a power problem as other have said, but your example MPU output shows the chip in an upside down position where it is just on the point of flicking between +180 and -180. What do you see if you have it right way up so it's sitting somewhere near zero? Do you get the same wild swings?
I ran your code and it works fine to get the values from the DMP.
Personally I find the way your code is structured unintuitive and hard to read (not your fault, you're just following the example). I like to have all the mpu.getXXX stuff inside a function that gets called from loop(). In my own self-balancing robot reading the MPU is just one of many tasks and I like to have them all called from loop(). Makes the code more readable.
Also you don't need the interrupt and a rolling average on the MPU output smooths things down a bit.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
bool blinkState = false;
bool toggle = 0;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
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
float rollArray [10] = {0,0,0,0,0,0,0,0,0,0}; // array to hold previous roll values for averaging
int numSamples = 4; // number of accelerometer readings over which to form a rolling average
double measuredRoll; // current averaged roll in degrees
double readMPU() { // Function to read data and return value in degrees
accelgyro.resetFIFO();
fifoCount = accelgyro.getFIFOCount();
while (fifoCount < packetSize) { // wait for correct available data length, should be a VERY short wait
fifoCount = accelgyro.getFIFOCount();
}
accelgyro.getFIFOBytes(fifoBuffer, packetSize); // read a packet from FIFO
fifoCount -= packetSize; // track FIFO count here in case there is > 1 packet available (lets us immediately read more without waiting for an interrupt)
accelgyro.dmpGetQuaternion(&q, fifoBuffer); // get all accelerometer data
accelgyro.dmpGetGravity(&gravity, &q);
accelgyro.dmpGetYawPitchRoll(ypr, &q, &gravity);
double sum = 0; // this section averages out the latest numSamples readings
double avg = 0; // value returned to PID loop is a rolling average updated every time there's new data
for (int i = numSamples-1; i > 0; i--){ // rollArray [0] holds most recent, rollArray[numSamples] holds oldest
rollArray[i] = rollArray[i-1]; // shuffle rollArray[3] into rollArray[4] etc. But not rollArray[0]...see below
sum = sum + rollArray[i];
}
rollArray[0] = ypr[1]; // load newest position in rollArray[] with current roll value
sum = sum + rollArray[0]; // and accumulate in sum
avg = sum / numSamples; // form the average
return avg * 180 / M_PI; // convert radians to degrees and return to PID control
}
void setup() {
delay(2000);
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line out if having compilation difficulties
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(accelgyro.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(F("Initializing DMP..."));
devStatus = accelgyro.dmpInitialize();
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
accelgyro.setDMPEnabled(true);
Serial.println(F("DMP ready! Waiting for data..."));
dmpReady = true;
packetSize = accelgyro.dmpGetFIFOPacketSize();
}
else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop() {
measuredRoll = readMPU();
Serial.println(measuredRoll);
delay(200); // slow down serial print; remove if full speed is required
}
