   // Delay
   delay(LOOP_DELAY);
 }
+
+
+void setupMPU(){
+  Wire.beginTransmission(0b1101000); //Beginn Transmission to MPU I2C address
+  Wire.write(0x6B); //open PWR_MGMT register
+  Wire.write(0b00000000); //Deactivating SLEEP-MODE in PWR_MGMT
+  Wire.endTransmission();
+  Wire.beginTransmission(0b1101000);
+  Wire.write(0x1B); //Open Gzro register
+  Wire.write(0x00000000); //Setting the gyro scale +/- 250deg./s 
+  Wire.endTransmission();
+}
+
+void calibrateMPU(){
+  recordGyroData();
+  prevX = gyroX;
+  prevY = gyroY;
+  prevZ = gyroZ;
+}
+
+
+void recordGyroData() {
+  Wire.beginTransmission(0b1101000); //I2C address of the MPU
+  Wire.write(0x43); //Starting register for Gyro Readings
+  Wire.endTransmission();
+  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
+  while(Wire.available() < 6);
+  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
+  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
+  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
+  processGyroData();
+}
+
+void processGyroData() {
+  rotX = gyroX / 131.0;
+  rotY = gyroY / 131.0; 
+  rotZ = gyroZ / 131.0;
+}
+


// INCLUDES
#include <Servo.h>

// SERVOS
Servo throttle;       // Speed

int pos3 = 0;
int pos4 = 0;
int pos5 = 0;

//orientation of MPU

int prevX = 0;
int prevY = 0;
int prevZ = 0;

int ByteReceived;   // store byte recieved via serial

