 IMU imu;
 
 void setup(){
-  Serial.begin(115200);
-  pinMode(12,INPUT);
-  pinMode(13,OUTPUT);
-  digitalWrite(12,HIGH);
-
   configSerialCommands();
   tEEPROM.configEpprom();
 

  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;
  }
  initBlController();
  initMotorStuff();
}

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

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