   // If so, assume actual gyro position and provide max servo value.
   
   // Invert Yaw values for my servo orientation.
+  // Comment out this line if your yaw is moving opposite the gyro.
   int invertedYaw = map(servoYaw, 0, 179, 179, 0);
 
   // Prevent the servos from panning farther than my head can rotate.
-  // May not be necessary after adding 2nd gyro and using relative angles.
+  // TODO: May not be necessary after adding 2nd gyro and using relative angles.
   if (invertedYaw > maxYaw) {
     invertedYaw = maxYaw;
   }

 *
 */
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 (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];
      }
    }
  } 
  
  MPUMath(); // Successful!  Do the math and show angles from both MPUs
  digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the LED on each cycle
  
}



    mpus[i].dmpGetYawPitchRoll(ypr, &q, &gravity);
  
    // Calculate readable angles
    Yaw[i] = (ypr[0] * 180.0 / M_PI);
    Pitch[i] = (ypr[1] *  180.0 / M_PI);

    //Calculate Roll if you need it.  We don't for the Predator project.
    //Roll[i] = (ypr[2] *  180.0 / M_PI) + servoAngleOffset;

    // Serial.print the current angle values of the gyro position.
    // DPRINTSTIMER(x) argument is the number of milliseconds between print events
    // Smaller numbers seem to give more reliable switching between MPU 0 an 1 but seem to cause crashing (Buffer overrun?)
      // DPRINTSTIMER(15) {
      //   DPRINTSFN(15, "\tValues for MPU :", i, 6, 1);
      //   DPRINTSFN(15, "\tYaw:", Yaw[i], 6, 1);
      //   DPRINTSFN(15, "\tPitch:", Pitch[i], 6, 1);
          //Write out Roll if you're using it.  We don't for the predator Project
          //   DPRINTSFN(15, "\tRoll:", Roll[i], 6, 1);
      //   DPRINTLN();
      // }
    
  }

  // Calculates the relative difference in angles between the two sensors.
  // Adds 90-degrees, value defined above, to center servos when diff between sensors is level.
  yawDiff = round((Yaw[1] - Yaw[0]) + servoAngleOffset);
  pitchDiff = round((Pitch[1] - Pitch[0]) + servoAngleOffset);

  // Timer - don't try and move the servos too frequently, they need time to get to the current position.
  //if (currentMillis >= prevMillis + servoDelay) {
    moveServos(yawDiff, pitchDiff); // TODO:  Change these values to the new diff'd values.
  //}

}

/**
