 // 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()
+
+// Will be used for switching readings so that we don't overrun the buffer
+int whichMPU = 0;
+
+
 
 
 

  }

  // Move the servos to the current yaw/pitch values
  yawServo.write(invertedYaw);
  pitchServo.write(servoPitch);

  // Reset the timer for comparison on the next loop
  prevMillis = currentMillis;

const int minYaw = 45;


// You must supply your gyro offsets here,
// Use MPU6050_calibration.ino found at:
//     https://forum.arduino.cc/index.php?PHPSESSID=h4c6487i42hbb7uh6rjk0eadp1&topic=446713.msg3073854#msg3073854
//     
// Calibration code also copied in my repo:  arduino/MPU6050/
// 
// Gyro 0 (head) calibrated at:   {-3074,  -2036,  1236,    63,    -16,     78}
//                           XA      YA      ZA     XG      YG      ZG
int mpuStaticOffsets[6] = {-3074,  -2036,  1236,    63,    -16,     78};
int mpuDynamOffsets[6]  = {-1143,  -1197,   989,   147,    -78,    -10};

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