 // ================================================================
 // ===                        MPU Math                          ===
 // ================================================================
-void MPUMath() {
-  mpu.dmpGetQuaternion(&q, fifoBuffer);
-  mpu.dmpGetGravity(&gravity, &q);
-  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-
-  // 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 = (ypr[0] * 180.0 / M_PI) + 90;
-  Pitch = (ypr[1] *  180.0 / M_PI) + 90;
-  Roll = (ypr[2] *  180.0 / M_PI) + 90;
-
-  /*
+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)
+    // 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, "\tYaw:", Yaw, 6, 1);
-    DPRINTSFN(15, "\tPitch:", Pitch, 6, 1);
-    DPRINTSFN(15, "\tRoll:", Roll, 6, 1);
-    DPRINTLN();
-  }
-  */
-
+//  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));
   }
+*/
 
 }
 


// Initialize gyro instance
// TODO:  Update this to initialize two gyros with +VDC/Ground arguments
MPU6050 mpu;

// Initialize servo instances
Servo yawServo;

}






// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================
volatile bool mpuInterrupt0 = false;     // indicates whether MPU interrupt pin has gone high
volatile bool mpuInterrupt1 = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady0() {
    mpuInterrupt0 = true;
}
void dmpDataReady1() {
    mpuInterrupt1 = true;
}



// ================================================================
// ===                      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) {
    // initialize device
    mpuStatic.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
