     invertedYaw = minYaw; 
   }
 
-  // Prevent the servos from tilting farther than my head can nod.
-  // May not be necessary after adding 2nd gyro and using relative angles.
+  // Prevent the servos from tilting farther than my head can pitch.
+  // TODO: May not be necessary after adding 2nd gyro and using relative angles.
   if (servoPitch > maxPitch) {
     servoPitch = maxPitch;
   }

// var to store the servo position when testing
int pos = 0;    

// vars for angle values.
float Yaw, Pitch, Roll;

// Will be used for timing events.
unsigned long currentMillis = 0;    // stores the value of millis() in each iteration of loop()
unsigned long prevMillis = 0;    // stores the value of millis() in each iteration of loop()

 *
 */
void GetDMP() { 
  if (currentMillis >= prevMillis + servoDelay) {
    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
  }
}


