 // ================================================================
 // ===                      MPU DMP SETUP                       ===
 // ================================================================
-int FifoAlive0 = 0; // tests if the interrupt is triggering
-int FifoAlive1 = 0; // tests if the interrupt is triggering
-//int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected
-// MPU control/status vars
-uint8_t mpuIntStatus0;   // holds actual interrupt status byte from MPU
-uint8_t devStatus0;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize0;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount0;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer0[64]; // FIFO storage buffer
-
-uint8_t mpuIntStatus1;   // holds actual interrupt status byte from MPU
-uint8_t devStatus1;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize1;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount1;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer1[64]; // FIFO storage buffer
-
-// orientation/motion vars
-Quaternion q0;           // [w, x, y, z]         quaternion container
-Quaternion q1;           // [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 gravity0;    // [x, y, z]            gravity vector
-VectorFloat gravity1;    // [x, y, z]            gravity vector
-//float euler[3];         // [psi, theta, phi]    Euler angle container
-float ypr0[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
-float ypr1[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
-//byte StartUP = 200; // lets get 200 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)
-
-
-
-/**
- * MPU6050Connect
- * =============================================================
- *
- * @Edited by Mike Muscato to enable dual MPU configuration 
- * @date   2017-07-02
- *
- */
-void MPU6050Connect(int connectMPU) {
-  static int MPUInitCntr = 0;
-
-  Serial.print(F("Initializing MPU ")); Serial.println(connectMPU);
-
-  if (connectMPU == 0) {
+uint8_t devStatus; // return status after each initialization operation (0 = success, !0 = error)
+
+//Containers for each MPU's communication
+int FifoAlive[2] = {0, 0}; // tests if the interrupt is triggering
+uint16_t packetSize[2];    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount[2];     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[2][64]; // FIFO storage buffer
+
+// orientation/motion vars 
+// (overwritten during each read/calculate cycle so they do not need to be duplicated for each MPU)
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+
+
+
+/**
+ * MPU6050 Connection / Initialization routine
+ * =============================================================
+ *
+ * @author Modified by Mike Muscato to enable dual MPU configuration 
+ * @date   2017-07-04
+ *
+ */
+void MPU6050Connect() {
+
+  for (int i = 0; i < 2; i++) {
+    Serial.print(F("Initializing MPU ")); Serial.println(i);
+    
+    static int MPUInitCntr = 0;  //Counter for looping and eventual initialization failure.
+    
     // initialize device
-    mpuStatic.initialize();
+    mpus[i].initialize();
     // load and configure the DMP
-    devStatus0 = mpuStatic.dmpInitialize();
-  }
-  else {
-    // initialize device
-    mpuDynam.initialize();
-    // load and configure the DMP
-    devStatus1 = mpuDynam.dmpInitialize();
-  }
-  
-  if (devStatus0 != 0) {
-    // ERROR!
-    // 1 = initial memory load failed
-    // 2 = DMP configuration updates failed
-    // (if it's going to break, usually the code will be 1)
-
-    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
-
-    MPUInitCntr++;
-
-    Serial.print(F("MPU connection Try #"));
-    Serial.println(MPUInitCntr);
-    Serial.print(F("DMP Initialization failed (code "));
-    Serial.print(StatStr[devStatus0]);
-    Serial.println(F(")"));
-
-    if (MPUInitCntr >= 10) return; //only try 10 times
-    delay(1000);
-    MPU6050Connect(connectMPU); // Lets try again
-    return;
-  }
-
-  if (connectMPU == 0) {
-    mpuStatic.setXAccelOffset(mpuStaticOffsets[0]);
-    mpuStatic.setYAccelOffset(mpuStaticOffsets[1]);
-    mpuStatic.setZAccelOffset(mpuStaticOffsets[2]);
-    mpuStatic.setXGyroOffset(mpuStaticOffsets[3]);
-    mpuStatic.setYGyroOffset(mpuStaticOffsets[4]);
-    mpuStatic.setZGyroOffset(mpuStaticOffsets[5]);
-
-    Serial.println(F("Enabling DMP on Static MPU..."));
-    mpuStatic.setDMPEnabled(true);
+    devStatus = mpus[i].dmpInitialize();
+    
+    if (devStatus != 0) {
+      
+      // ERROR Checking!
+      // 1 = initial memory load failed
+      // 2 = DMP configuration updates failed
+      // (if it's going to break, usually the code will be 1)
+      
+      char *StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
+
+      MPUInitCntr++;
+
+      Serial.print(F("MPU")); Serial.print(i); Serial.print(F(" connection Try #")); Serial.println(MPUInitCntr);
+      Serial.print(F("DMP Initialization failed (code ")); Serial.print(StatStr[devStatus]); Serial.println(F(")"));
+      
+      if (MPUInitCntr >= 10) return; //only try 10 times
+      delay(1000);
+      MPU6050Connect(); // Lets try again
+      return;
+    }
+
+    //Set offset values for each of our MPUs from our array.
+    mpus[i].setXAccelOffset(mpuOffsets[i][0]);
+    mpus[i].setYAccelOffset(mpuOffsets[i][1]);
+    mpus[i].setZAccelOffset(mpuOffsets[i][2]);
+    mpus[i].setXGyroOffset(mpuOffsets[i][3]);
+    mpus[i].setYGyroOffset(mpuOffsets[i][4]);
+    mpus[i].setZGyroOffset(mpuOffsets[i][5]);
+
+    Serial.print(F("Enabling DMP on MPU")); Serial.println(i);
+    mpus[i].setDMPEnabled(true);
 
     // enable Arduino interrupt detection
-    Serial.println(F("Enabling interrupt detection on Static MPU (Arduino external interrupt pin 2 on the Uno)..."));
-    Serial.print(F("mpuStatic.getInterruptDrive=  ")); Serial.println(mpuStatic.getInterruptDrive());
-    attachInterrupt(0, dmpDataReady0, RISING); //pin 2 on the Uno
-    mpuIntStatus0 = mpuStatic.getIntStatus();
-    // get expected DMP packet size for later comparison
-    packetSize0 = mpuStatic.dmpGetFIFOPacketSize();
+    int intPin = i + 2;
+    Serial.print(F("Enabling interrupt detection on MPU")); Serial.print(i); Serial.print(F("(Arduino external interrupt pin ")); Serial.print(intPin); Serial.println(F(" on the Uno)..."));
+    Serial.print(F("mpu")); Serial.print(i); Serial.print(F(".getInterruptDrive=  ")); Serial.println(mpus[i].getInterruptDrive());  //Current interrupt drive mode (0=push-pull, 1=open-drain)
+    
+    //Intentional duplication. Interrupt routine cannot pass args. 
+    if (i == 0) {
+      attachInterrupt(i, dmpDataReady0, RISING);
+    }
+    else {
+      attachInterrupt(i, dmpDataReady1, RISING);
+    }
+    
+    packetSize[i] = mpus[i].dmpGetFIFOPacketSize();  // get expected DMP packet size for later comparison
     delay(1000); // Let it Stabalize
-    mpuStatic.resetFIFO(); // Clear fifo buffer
-    mpuStatic.getIntStatus();
-    mpuInterrupt0 = false; // wait for next interrupt
-  }
-  else {
-    mpuDynam.setXAccelOffset(mpuDynamOffsets[0]);
-    mpuDynam.setYAccelOffset(mpuDynamOffsets[1]);
-    mpuDynam.setZAccelOffset(mpuDynamOffsets[2]);
-    mpuDynam.setXGyroOffset(mpuDynamOffsets[3]);
-    mpuDynam.setYGyroOffset(mpuDynamOffsets[4]);
-    mpuDynam.setZGyroOffset(mpuDynamOffsets[5]);
-
-    Serial.println(F("Enabling DMP on Dyanmic MPU..."));
-    mpuDynam.setDMPEnabled(true);
-
-    // enable Arduino interrupt detection
-    Serial.println(F("Enabling interrupt detection on Dynamic MPU (Arduino external interrupt pin 3 on the Uno)..."));
-    Serial.print(F("mpuDynam.getInterruptDrive=  ")); Serial.println((mpuDynam.getInterruptDrive()));
-    attachInterrupt(1, dmpDataReady1, RISING); //pin 3 on the Uno
-    mpuIntStatus1 = mpuDynam.getIntStatus();
-    // get expected DMP packet size for later comparison
-    packetSize1 = mpuDynam.dmpGetFIFOPacketSize();
-    delay(1000); // Let it Stabalize
-    mpuDynam.resetFIFO(); // Clear fifo buffer
-    mpuDynam.getIntStatus();
-    mpuInterrupt1 = false; // wait for next interrupt
-  }
-
-}
-
-
-
-// ================================================================
-// ===                    MPU DMP Get Data                      ===
-// ================================================================
-void GetDMP(int dmpMPU) { // Best version I have made so far
-  //Serial.println("dmpMPU");
-  // Serial.println(F("FIFO interrupt at:"));
-  // Serial.println(micros());
-
-  if (dmpMPU == 0) {
-    static unsigned long LastGoodPacketTime;
-    mpuInterrupt0 = false;
-    FifoAlive0 = 1;
-    fifoCount0 = mpuStatic.getFIFOCount();
-    if ((!fifoCount0) || (fifoCount0 % packetSize0)) { // we have failed Reset and wait till next time!
-      digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
-        mpuStatic.resetFIFO();// clear the buffer and start over
+    mpus[i].resetFIFO(); // Clear fifo buffer    
+    mpuInterrupt[i] = false; // wait for next interrupt
+    
+  }
+
+}
+
+
+
+/**
+ * GetDMP Reads the data from our MPUs and stores it in each mpus instance.
+ * =============================================================
+ *
+ * @author Modified by Mike Muscato to enable dual MPU configuration 
+ * @date   2017-07-04
+ *
+ */
+void GetDMP() { 
+
+  for (int i = 0; i < 2; i++) { 
+    mpuInterrupt[i] = false;
+    FifoAlive[i] = 1;
+    fifoCount[i] = mpus[i].getFIFOCount();
+    
+    if ((!fifoCount[i]) || (fifoCount[i] % packetSize[i])) { // we have failed Reset and wait till next time!
+      digitalWrite(LED_PIN, LOW); // lets turn off the blinking LED so we can see we are failing.
+        mpus[i].resetFIFO();// clear the buffer and start over
     } 
     else {
-      while (fifoCount0  >= packetSize0) { // Get the packets until we have the latest!
-          mpuStatic.getFIFOBytes(fifoBuffer0, packetSize0); // lets do the magic and get the data
-          fifoCount0 -= packetSize0;
+      while (fifoCount[i]  >= packetSize[i]) { // Get the packets until we have the latest!
+          mpus[i].getFIFOBytes(fifoBuffer[i], packetSize[i]); // lets do the magic and get the data
+          fifoCount[i] -= packetSize[i];
       }
-      LastGoodPacketTime = millis();
-      MPUMath(dmpMPU); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
-      digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
     }
-  }
-  else {
-    static unsigned long LastGoodPacketTime;
-    mpuInterrupt1 = false;
-    FifoAlive1 = 1;
-    fifoCount1 = mpuDynam.getFIFOCount();
-    if ((!fifoCount1) || (fifoCount1 % packetSize1)) { // we have failed Reset and wait till next time!
-      digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
-        mpuDynam.resetFIFO();// clear the buffer and start over
-    } 
-    else {
-      while (fifoCount1  >= packetSize1) { // Get the packets until we have the latest!
-          mpuDynam.getFIFOBytes(fifoBuffer1, packetSize1); // lets do the magic and get the data 
-          fifoCount1 -= packetSize1;
-      }
-      LastGoodPacketTime = millis();
-      MPUMath(dmpMPU); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
-      digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
-    } 
-  }
-}
-
-
-// ================================================================
-// ===                        MPU Math                          ===
-// ================================================================
-void MPUMath(int mathMPU) {
-  //Serial.println("mathMPU");
-  if (mathMPU == 0) {
-    mpuStatic.dmpGetQuaternion(&q0, fifoBuffer0);
-    mpuStatic.dmpGetGravity(&gravity0, &q0);
-    mpuStatic.dmpGetYawPitchRoll(ypr0, &q0, &gravity0);
-
-    // Add 90-degrees to center servos when sensor is level (e.g. 0 degrees)
+  } 
+  
+  MPUMath(); // Successful!  Do the math and show angles from both MPUs
+  digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the LED on each cycle
+  
+}
+
+
+
+/**
+ * MPUMath Calculates angles of MPUs from raw sensor data
+ * =============================================================
+ *
+ * @author Modified by Mike Muscato to enable dual MPU configuration 
+ * @date   2017-07-04
+ *
+ */
+void MPUMath() {
+  for (int i = 0; i < 2; i++) {
+    // Get the raw data from the sensor
+    mpus[i].dmpGetQuaternion(&q, fifoBuffer[i]);
+    mpus[i].dmpGetGravity(&gravity, &q);
+    mpus[i].dmpGetYawPitchRoll(ypr, &q, &gravity);
+  
+    // Calculate readable angles
+    // Adds 90-degrees to center servos when sensor is level (e.g. 0 degrees)
     // TODO: Move the +90 gyro/servo leveling magic number to another method, 
     // ...it doesn't belong here and will cause readability problems later.
-    Yaw = (ypr0[0] * 180.0 / M_PI) + 90;
-    Pitch = (ypr0[1] *  180.0 / M_PI) + 90;
-    Roll = (ypr0[2] *  180.0 / M_PI) + 90;
-  }
-  else {
-    mpuDynam.dmpGetQuaternion(&q1, fifoBuffer1);
-    mpuDynam.dmpGetGravity(&gravity1, &q1);
-    mpuDynam.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);
-
-    // Add 90-degrees to center servos when sensor is level (e.g. 0 degrees)
-    // TODO: Move the +90 gyro/servo leveling magic number to another method, 
-    // ...it doesn't belong here and will cause readability problems later.
-    Yaw = (ypr1[0] * 180.0 / M_PI) + 90;
-    Pitch = (ypr1[1] *  180.0 / M_PI) + 90;
-    Roll = (ypr1[2] *  180.0 / M_PI) + 90;
-  }
-
-
-  Serial.print(F("\tValues for MPU :"));Serial.print(mathMPU);
-  Serial.print(F("\tYaw:"));Serial.print(Yaw);
-  Serial.print(F("\tPitch:"));Serial.print(Pitch);
-  Serial.print(F("\tRoll:"));Serial.println(Roll);
-  
-  // Serial.print the current actual values of the gyro position.
-//  DPRINTSTIMER(100) {
-//    DPRINTSFN(15, "\tValues for MPU :", mathMPU, 6, 1);
-//    DPRINTSFN(15, "\tYaw:", Yaw, 6, 1);
-//    DPRINTSFN(15, "\tPitch:", Pitch, 6, 1);
-//    DPRINTSFN(15, "\tRoll:", Roll, 6, 1);
-//    DPRINTLN();
-//  }
-  
-
-/*
-  // Timer - don't try and move the servos too frequently, they need time to get to the current position.
-  // TODO: Remove delay magic number and move to global var for tuning mechanical
-  if (currentMillis >= prevMillis + 15) {
-    moveServos(round(Yaw), round(Pitch));
-  }
-*/
-
-}
-
+    Yaw = (ypr[0] * 180.0 / M_PI) + 90;
+    Pitch = (ypr[1] *  180.0 / M_PI) + 90;
+    Roll = (ypr[2] *  180.0 / M_PI) + 90;
+
+    //TODO: Add YPR values to temp var for each MPU, so that we can diff them.
+
+    // Serial.print the current angle values of the gyro position.
+    // DPRINTSTIMER(x) argument is the number of milliseconds between print events
+    // Smaller numbers give more reliable switching between MPU 0 an 1 but seem to cause crashing (Buffer overrun?)
+    //    DPRINTSTIMER(1) {
+    //      DPRINTSFN(15, "\tValues for MPU :", i, 6, 1);
+    //      DPRINTSFN(15, "\tYaw:", Yaw, 6, 1);
+    //      DPRINTSFN(15, "\tPitch:", Pitch, 6, 1);
+    //      DPRINTSFN(15, "\tRoll:", Roll, 6, 1);
+    //      DPRINTLN();
+    //    }
+    
+  }
+
+    // Timer - don't try and move the servos too frequently, they need time to get to the current position.
+    // TODO: Remove delay magic number and move to global var for tuning mechanical response
+    if (currentMillis >= prevMillis + 15) {
+      moveServos(round(Yaw), round(Pitch)); // TODO:  Change these values to the new diff'd values.
+    }
+}
 
 /**
  * Move the servos to the current gyro postion


// Standard included arduino servo library
#include "Servo.h"

// Initialize gyro instance
// TODO:  Update this to initialize two gyros with +VDC/Ground arguments
MPU6050 mpuStatic(0x68);
MPU6050 mpuDynam(0x69);

// Initialize servo instances
Servo yawServo;
Servo pitchServo;


// Comment to turn off Serial printing
#define DEBUG  

 * @return {void}
 */
void setup() {
  Serial.begin(38400);
  while (!Serial);  // Wait for the connection to be established?

  // Run MPU initializations
  Serial.println(F("i2cSetup"));
  i2cSetup();
  Serial.println(F("MPU6050 Connection Routine"));
  MPU6050Connect();
  Serial.println(F("Setup complete"));

  pinMode(LED_PIN, OUTPUT);
