diff --git a/include/Vehicle.h b/include/Vehicle.h
index 0c266dc..4cc5681 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -35,6 +35,7 @@ struct Vehicle {
   double yError, yPrevError;
   double pError, pPrevError;
   double i_yError, i_pError = 0;
+  int pidTest = 0;
 
   double simTime;
   int stepSize;
diff --git a/include/sim.h b/include/sim.h
index cf611b7..abf0e14 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -190,19 +190,25 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
 
   // Make sure we start reacting when we start burning
   if (State.thrust > 0.01) {
+
+    State.yError = State.yaw;
+    State.pError = State.pitch;
+
     // Integral of Error
-    State.i_yError = integral(State.yaw, State.i_yError, State.stepSize);
-    State.i_pError = integral(State.pitch, State.i_pError, State.stepSize);
+    State.i_yError = integral(State.yError, State.i_yError, State.stepSize);
+    State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
 
     // Derivative of Error
-    double d_yError = derivative(State.yaw, PrevState.yaw, State.stepSize);
-    double d_pError = derivative(State.pitch, PrevState.pitch, State.stepSize);
+    double d_yError =
+        derivative(State.yError, PrevState.yError, State.stepSize);
+    double d_pError =
+        derivative(State.pError, PrevState.pError, State.stepSize);
 
     // PID Function - it says LQR but this is just so that it gets passed to the
     // TVC block properly
-    State.LQRx = (State.Kp * State.yaw + State.Ki * State.i_yError +
+    State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
                   State.Kd * d_yError);
-    State.LQRy = (State.Kp * State.pitch + State.Ki * State.i_pError +
+    State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
                   State.Kd * d_pError);
 
   } else {
diff --git a/input.csv b/input.csv
index d4c67d6..a085a5b 100644
--- a/input.csv
+++ b/input.csv
@@ -15,6 +15,6 @@ Vehicle Height,0.53,m
 Vehicle Radius,0.05,m
 Moment Arm,0.15,m
 Sim Step Size,1,ms
-Kp,-0.25,x
-Ki,0.4,x
-Kd,1,x
+Kp,-1,x
+Ki,-1,x
+Kd,-1,x
diff --git a/src/main.cpp b/src/main.cpp
index 6ad6716..83fddd3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -66,10 +66,6 @@ int main() {
   State.Ki = varValueVec[18];
   State.Kd = varValueVec[19];
 
-  std::cout << State.Kp << "\n";
-  std::cout << State.Ki << "\n";
-  std::cout << State.Kd << "\n";
-
   // Other Properties
   State.burntime = varValueVec[12];                             // [s]
   State.massPropellant = varValueVec[11];                       // [kg]