   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 pitchPID = ComputePID(DT_INT_MS, DT_INT_INV,-1*pitchInt, setAngle*1000, &pitchErrorSum, &pitchErrorOld,pitchP,pitchI,pitchD);//500,20,5,100 pwr,~10.7V
   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(1, pitchPID, (uint16_t)pitchMotorPower);
   MoveMotorPosSpeed(0, pitchPIDOutput,60);
   MoveMotorPosSpeed(1, rollPIDOutput,90);
  
-  if(subTick %8 == 0){
+  if(subTick % SUBTICK_FREQ == (SUBTICK_FREQ-1)){
     if(outputAngle){
       Serial.print("PRA ");
       Serial.print(pitchAngle);
       Serial.print(" ");
       Serial.println(rollAngle);
+      subTick = 0;
     }
-    //setAngle = (analogRead(A0) - 512)/10;
-    //Serial.println(rollInt);
   }
   subTick++;
-  subTick = subTick %256;
 }
 
 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)

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

