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


void setup() {
  Serial.begin(9600);
#ifdef TEST_CONTROLS
  ManualControl = true;
#else

ulong TimeSinceStart;
ulong TimeDelta;
bool ManualControl;

double SetYaw, IputYaw, OputYaw;
double SetRoll, IputRoll, OputRoll;
double SetPitch, IputPitch, OputPitch;
double SetThro, IputThro, OputThro;

PID YawPid(&IputYaw, &OputYaw, &SetYaw,5,10,10,DIRECT);
PID RollPid(&IputRoll, &OputRoll, &SetRoll,5,10,10,DIRECT);
PID PitchPid(&IputPitch, &OputPitch, &SetPitch,5,10,10,DIRECT);
PID ThroPid(&IputThro, &OputThro, &SetThro,5,10,10,DIRECT);


Adafruit_L3GD20 gyro;

