 
 
 void setup(){
-        Serial.begin(115200);
-        pinMode(12,INPUT);
-        pinMode(13,OUTPUT);
-        digitalWrite(12,HIGH);
-        //timer2.setup();
-        configSerialCommands();
-        tEEPROM.configEpprom();
-        //initRCDecode();
-        tEEPROM.readPID(pitchPID,rollPID);
-        tEEPROM.initReadMotorPower(&pitchMotorPower);
+  Serial.begin(115200);
+  pinMode(12,INPUT);
+  pinMode(13,OUTPUT);
+  digitalWrite(12,HIGH);
+
+  configSerialCommands();
+  tEEPROM.configEpprom();
+
+  tEEPROM.readPID(pitchPID,rollPID);
+  tEEPROM.initReadMotorPower(&pitchMotorPower);
         
 	initSensors();
-        //Serial.println("Calibrating MPU6050");
-        for(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);
-	  }
+  //Serial.println("Calibrating MPU6050");
+  for(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;
-        }
-        //Serial.println("Done Calibrating MPU6050");
+  }
+  initBlController();
+  initMotorStuff();
+}
 
-        initBlController();
-        initMotorStuff();
-}
-void loop()
-{
-        SCMD.readSerial();
+void loop(){
+  SCMD.readSerial();
 	Gyro_getADC(); // read raw gyro data
 	ACC_getADC(); // read raw accel data
-	IMU();
+	
 
-        pitchInt = pitchAngle *1000;
-        rollInt = rollAngle *1000;
-        
-        int32_t pitchP = pitchPID[0];
-        int32_t pitchI = pitchPID[1];
-        int32_t pitchD = pitchPID[2];
-        
-        int32_t rollP = rollPID[0];
-        int32_t rollI = rollPID[1];
-        int32_t rollD = rollPID[2];
-        
-        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 pitchPID = ComputePID(DT_INT_MS, DT_INT_INV,pitchInt, setAnglePitchLPF*1000, &pitchErrorSum, &pitchErrorOld,pitchP,pitchI,pitchD);//500,20,5,100 pwr,~10.7V
-        int rollPID = 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, pitchPID,60);
-        MoveMotorPosSpeed(1, rollPID,90);
-       
-        if(subTick %8 == 0){
-          if(outputAngle){
-            Serial.print("PRA ");
-            Serial.print(pitchAngle);
-            Serial.print(" ");
-            Serial.println(rollAngle);
-          }
-          //setAngle = (analogRead(A0) - 512)/10;
-          //Serial.println(rollInt);
-        }
-        subTick++;
-        subTick = subTick %256;
+  pitchInt = pitchAngle *1000;
+  rollInt = rollAngle *1000;
+  
+  int32_t pitchP = pitchPID[0];
+  int32_t pitchI = pitchPID[1];
+  int32_t pitchD = pitchPID[2];
+  
+  int32_t rollP = rollPID[0];
+  int32_t rollI = rollPID[1];
+  int32_t rollD = rollPID[2];
+  
+  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(outputAngle){
+      Serial.print("PRA ");
+      Serial.print(pitchAngle);
+      Serial.print(" ");
+      Serial.println(rollAngle);
+    }
+    //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)

#include "SerialCommand.h"
#include "variables.h"
#include "pinChangeInt.h"
#include <avr/pgmspace.h>



#define PWM_32KHZ_PHASE
#define PI 3.14159265         // approx of Pi which program uses.



static uint16_t calibratingG;         //used for initializing Acc calibration

static long calSum;                   //used for gyro calibration      10
static float gyroZero[3] = {0,0,0};   //stores values for gyro calibration
