 // ================================================================
 // ===                      MPU DMP SETUP                       ===
 // ================================================================
-// MPU control/status vars
-
-//This isn't used anywhere, I think we can get rid of it.
-//uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
-
-uint8_t devStatus;      // return status after each initialization operation (0 = success, !0 = error)
+uint8_t devStatus; // return status after each initialization operation (0 = success, !0 = error)
 
 //Containers for each MPU's communication
 int FifoAlive[2] = {0, 0}; // tests if the interrupt is triggering

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================
volatile bool mpuInterrupt[2] = {false, false};     // indicates whether MPU interrupt pin has gone high

//Intentional duplication. Interrupt routine cannot pass args. 
void dmpDataReady0() {

void MPUMath() {
  for (int i = 0; i < 2; i++) {

    // Get the raw data from the sensor
    mpus[i].dmpGetQuaternion(&q, fifoBuffer[i]);
    mpus[i].dmpGetGravity(&gravity, &q);
    mpus[i].dmpGetYawPitchRoll(ypr, &q, &gravity);
  
    // Calculate readable angles
    Yaw = (ypr[0] * 180.0 / M_PI);
    Pitch = (ypr[1] *  180.0 / M_PI);
    Roll = (ypr[2] *  180.0 / M_PI);
