 // 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()

#include "Servo.h"

// Comment to turn off Serial printing
#define DEBUG  

// DEBUG serial printing macros
#ifdef DEBUG

 *
 */
void MPUMath() {
  // vars for angle values.
  float Yaw[2], Pitch[2]; 
  int yawDiff, pitchDiff;

  //Add Roll if you need it, we don't for the Predator project; 
  //float Roll[2];
  //int rollDiff;

  for (int i = 0; i < 2; i++) {
    // Get the raw data from the sensor
    mpus[i].dmpGetQuaternion(&q, fifoBuffer[i]);
