  *
  */
 void MPUMath() {
+  // vars for angle values.
+  float Yaw[2], Pitch[2]; 
+  int yawDiff, pitchDiff;
+
+  //Add Roll if you need it, we don't for the Predator project; 
+  //float Roll[2];
+  //int rollDiff;
+
   for (int i = 0; i < 2; i++) {
     // Get the raw data from the sensor
     mpus[i].dmpGetQuaternion(&q, fifoBuffer[i]);

    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 = (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.
    }
}

/**

#include "Servo.h"

// Comment to turn off Serial printing
//#define DEBUG  

// DEBUG serial printing macros
#ifdef DEBUG
