Second part:
mpu.initialize();
// verify connection
//mySerial.println(F("Testing device connections..."));
mySerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
//COULD BE HERE INT
// wait for ready
//mySerial.println(F("\nSend any character to begin DMP programming and demo: "));
while (mySerial.available() && mySerial.read()); // empty buffer //MOD "mySerial"
while (!mySerial.available()); // wait for data //MOD
while (mySerial.available() && mySerial.read()); // empty buffer again //MOD
// load and configure the DMP
//mySerial.println(F("Sarom Chhin 2014")); //BT
//mySerial.println(F("Senior Project; 409B Independent Studies.")); //BT
//delay(4000); //BT.
//mySerial.println(F("Starting gyro+accel module!")); //BT.
//delay(500); //BT.
//mySerial.println(F("(Initializing DMP...)"));
//delay(500); //BT.
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1988); // 1988 best so far, w z=220's. Default = 1788.
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
//mySerial.println(F("(Enabling DMP...)"));
//delay(500); //BT.
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//mySerial.println(F("External interrupt detected.")); //BT. Default: ""Enabling interrupt detection (Arduino external interrupt 0)..."));
//delay(500); //BT.
attachInterrupt(0, dmpDataReady, RISING); //MOD. "0" is the interrupt (int.0 = pin2 for UNO),
//attachInterrupt(0, dmpDataReady, CHANGE); //BT. Default is "RISING".
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
//mySerial.println(F("DMP ready! Waiting for first interrupt..."));
//mySerial.println(F("____________________________")); //BT
//mySerial.println(F(" Yaw Pitch Roll")); //BT.
//mySerial.println(F(" X Y Z")); //BT.
//mySerial.println(F("----------------------------")); //BT.
//delay(4000); //BT.
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)
mySerial.print(F("DMP Initialization failed (code "));
mySerial.print(devStatus);
mySerial.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();
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);
mySerial.print("YPR\t");
mySerial.print(ypr[0] * 180/M_PI);
mySerial.print("\t");
mySerial.print(ypr[1] * 180/M_PI);
mySerial.print("\t");
mySerial.println(ypr[2] * 180/M_PI);
delay(500); //BT.
#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);
mySerial.print("areal\t");
mySerial.print(aaReal.x);
mySerial.print("\t");
mySerial.print(aaReal.y);
mySerial.print("\t");
mySerial.println(aaReal.z);
delay(200); //BT.
#endif
*/
#ifdef OUTPUT_READABLE_WORLDACCEL
// 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);
mySerial.print("\t");
//mySerial.print("X");
//mySerial.print(aaWorld.x);
mySerial.print(abs(aaWorld.x));
mySerial.print("\t");
//mySerial.print("Y");
//mySerial.print(aaWorld.y);
mySerial.print(abs(aaWorld.y));
mySerial.print("\t");
//mySerial.print("Z");
//mySerial.println(aaWorld.z - 220); //BT. -220 Calibrated to board being parallel to Earth.
mySerial.println(abs(aaWorld.z -360));
delay(100); //BT.
#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);
}
}