     const int CONTROLLER_MIN = 0;
 
     ControllerInput Input = GetControllerInput();
-    int Throttle = Input.Throttle;
-    int Roll     = Input.Roll;
-    int Pitch    = Input.Pitch;
-    int Yaw      = Input.Yaw;
+    IputThro = Input.Throttle;
+    IputRoll = Input.Roll;
+    IputPitch    = Input.Pitch;
+    IputYaw      = Input.Yaw;
+
+    YawPid.Compute();
+    PitchPid.Compute();
+    RollPid.Compute();
+    ThroPid.Compute();
+
+    int Yaw=OputYaw;
+    int Roll=OputRoll;
+    int Pitch=OputPitch;
+    int Throttle=OputThro;
     
     Throttle = map(Throttle, CONTROLLER_MIN, CONTROLLER_MAX, SERVO_OFF, SERVO_FULL);
     Roll     = map(Roll,     CONTROLLER_MIN, CONTROLLER_MAX, SERVO_OFF, SERVO_FULL);

ulong TimeSinceStart;
ulong TimeDelta;
bool ManualControl;

Adafruit_L3GD20 gyro;



void setup() {
  Serial.begin(9600);

  /*set up PID */
  YawPid.SetOutputLimits(0,360);// return in degree
  PitchPid.SetOutputLimits(0,360);
  RollPid.SetOutputLimits(0,360);
  ThroPid.SetOutputLimits(1000,2000);//limit max thro output to 
  /*end set up PID*/
  
#ifdef TEST_CONTROLS
  ManualControl = true;
#else
