 	Gyro_getADC(); // read raw gyro data
 	ACC_getADC(); // read raw accel data
   imu.calculate();
-	
 
   pitchInt = pitchAngle *1000;
   rollInt = rollAngle *1000;

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

  setAnglePitchLPF = setAnglePitchLPF * (1.0f - (1.0f/PS_LPF_FACTOR)) + setAnglePitch * (1.0f/PS_LPF_FACTOR);
  setAngleRollLPF = setAngleRollLPF * (1.0f - (1.0f/PS_LPF_FACTOR)) + setAngleRoll * (1.0f/PS_LPF_FACTOR);
  
  int pitchPIDOutput = ComputePID(DT_INT_MS, DT_INT_INV,pitchInt, setAnglePitchLPF*1000, &pitchErrorSum, &pitchErrorOld,pitchP,pitchI,pitchD);//500,20,5,100 pwr,~10.7V
  int rollPIDOutput = ComputePID(DT_INT_MS, DT_INT_INV,-1*rollInt, setAngleRollLPF*1000, &rollErrorSum, &rollErrorOld,rollP,rollI,rollD);//500,20,5,100 pwr,~10.7V
  
  MoveMotorPosSpeed(0, pitchPIDOutput,60);
  MoveMotorPosSpeed(1, rollPIDOutput,90);
 
  if(subTick % SUBTICK_FREQ == (SUBTICK_FREQ-1)){
    if(outputAngle){
      Serial.print("PRA ");
      Serial.print(pitchAngle);
      Serial.print(" ");
      Serial.println(rollAngle);
      subTick = 0;
    }
  }
  subTick++;
}

int32_t ComputePID(int32_t DTms, int32_t DTinv, int32_t in, int32_t setPoint, int32_t *errorSum, int32_t *errorOld, int32_t Kp, int16_t Ki, int32_t Kd)
