   tEEPROM.initReadMotorPower(&pitchMotorPower);
         
 	initSensors();
-  //Serial.println("Calibrating MPU6050");
-  for(uint8_t axis = 0; axis < 3; axis++){ //calculate offsets to zeros gyros 
-    for(int i = 0; i < 400; i++){ //averages 400 readings for each axis
-      Gyro_getADC();
-      calSum += gyroADC[axis];
-      delay(1);
-    }
-	  gyroZero[axis] = calSum/400.f;
-	  calSum = 0;
-  }
+  gyroCalibration();
+  
   initBlController();
   initMotorStuff();
 }

	Gyro_getADC(); // read raw gyro data
	ACC_getADC(); // read raw accel data
  imu.calculate();
	

  pitchInt = pitchAngle *1000;
  rollInt = rollAngle *1000;

IMU imu;

void setup(){
  configSerialCommands();
  tEEPROM.configEpprom();

