   
   Serial.begin(9600);                 // Serial only necessary if in DEBUG mode
   Serial.flush();
-  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
   
   #endif
 }

    vd = ESC_MIN;
  
  }
  
  #ifdef DEBUG
  //Serial.println(ch2);
  //Serial.print("A: "+String(va)+" B: "+String(vb)+" C: "+String(vc)+" D: "+String(vd));
  /*Serial.print("AC: ");
  Serial.print(bal_ac);
  Serial.print(" BD: ");
  Serial.print(bal_bd);
  Serial.println("");*/
  #endif
  
}


  ypr[0] = ypr[0] * 180/M_PI;
  ypr[1] = ypr[1] * 180/M_PI;
  ypr[2] = ypr[2] * 180/M_PI;
 
  pitchReg.Compute();
  rollReg.Compute();
  yawReg.Compute();

}

