This code works great!!!
Wiring:
You must hook up the INT to pin 2 on your Arduino!
Use the Library found at i2cdevlib/Arduino/MPU6050 at master · jrowberg/i2cdevlib · GitHub
Run The MPU6050_calibration.ino (attached below) and follow the instructions in the serial monitor: set it to 115200 baud
You will get the Calibration for your MPU6050. Every MPU6050 is different!
Look for these lines of code in the MPU6050_Latest_code.ino sketch:
// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino
// -4232 -706 1729 173 -94 37
// XA YA ZA XG YG ZG
int MPUOffsets[6] = { -4232, -706, 1729, 173, -94, 37};
put your offsets you got from the calibration here.
Note: I have custom Debug Print Macros These are basically enhanced Serial.print() for displaying information.
#define DPRINTSTIMER(t) for (static unsigned long SpamTimer; (unsigned long)(millis() - SpamTimer) >= (t); SpamTimer = millis()) // Spam Timer
#define DPRINTSFN(StrSize,Name,...) {char S[StrSize];Serial.print("\t");Serial.print(Name);Serial.print(" "); Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Percision <<< Prints floats as a formated string with description
#define DPRINTLN(...) Serial.println(__VA_ARGS__) // Simple Println()
In the function MPUMath() I am calculating the Yaw, Pitch and Roll angles that you are looking for.
Note: The MPUMath triggers by interrupt (pin 2) about every 10 milliseconds. your loop() code can't have delays more than 8~9 ms or the FIFO(First in First out) buffer in the MPU6050 will become corrupted and your readings will fail. avoid using delay() in the loop at all costs. look up "Blink without delay" to learn how to do this.
void MPUMath() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Yaw = (ypr[0] * 180.0 / M_PI);
Pitch = (ypr[1] * 180.0 / M_PI);
Roll = (ypr[2] * 180.0 / M_PI);
DPRINTSTIMER(100) {
DPRINTSFN(15, "\tYaw:", Yaw, 6, 1); // \t is the tab character // fancy Serial.Print
DPRINTSFN(15, "\tPitch:", Pitch, 6, 1); // how to use DPRINTSFN() 15: USING 15 CHARACTERS MAX for the number to string conversion, "\tPitch": displayed name, Pitch: Floating point value to display, 6: using 6 characters in the conversion, 1: trim the float to 1 decimal place
DPRINTSFN(15, "\tRoll:", Roll, 6, 1); //DPRINTSFN() Will display any floating point number with simple and space everything nicely
DPRINTLN(); // Simple New Line
}
}
alternatly you might want other information from the DMP (Digital Motion Processing) available from the MPU6050
// 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);
// 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);
// 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);
// 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);
// 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.println(aaWorld.z);
// 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
MPU6050_Latest_code.ino (7.4 KB)
MPU6050_calibration.ino (7.64 KB)
Thats right you are using a different chip than the atmega328p I didn't realize that the ATmega32U4 uses pins 2&3 for i2c communications