   }
   */
 
+  // 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
   if (currentMillis >= prevMillis + 15) {
     moveServos(round(Yaw), round(Pitch));
   }
 
-
-}
+}
+
 
 /**
- * Move the servos to the gyro postion
+ * Move the servos to the current gyro postion
  * =============================================================
  *
  * @author Mike Muscato

    servoPitch = minPitch;  
  }

  DPRINTSTIMER(100) {
    DPRINTSFN(15, "\tServo - Yaw:", servoYaw, 6, 1);
    DPRINTSFN(15, "\tServo - invertYaw:", invertedYaw, 6, 1);

// ================================================================
// ===                        MPU Math                          ===
// ================================================================
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

  // Add 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;

  /*
  // Serial.print the current actual values of the gyro position.
  DPRINTSTIMER(100) {
    DPRINTSFN(15, "\tYaw:", Yaw, 6, 1);
    DPRINTSFN(15, "\tPitch:", Pitch, 6, 1);
