diff --git a/main.cpp b/main.cpp
index 24b9178..c4cd8fa 100644
--- a/main.cpp
+++ b/main.cpp
@@ -28,6 +28,8 @@ using namespace math;
 
 EventQueue queue;
 
+// Serial port (Servo Outputs)
+Serial serialMotorOutputs(PA_2, PA_3, 115200);
 
 // MPU setup
 SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
@@ -50,7 +52,7 @@ Timer timer;
 controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
 
 // TODO: Figure out some good values
-controllerPD angleControl(25.0, 35.25, 400);
+controllerPD angleControl(10.0, 146.0, 400);
 // Alternatively try controllerPD2 which has another Dterm implementation
 
 // Draft of some control function
@@ -66,6 +68,7 @@ void controlFunc()
 
     // Retrieve IMU angle (Only x-axis implemented for now)
     float angleX = imuFusion.getAngle(dT);
+    float forceX = sin(angleX/180*PI);
     // Reset anything left in the IMU FIFO queue
     imu.fifo_reset();
 
@@ -75,10 +78,14 @@ void controlFunc()
     {
         controlOutput = 0.0f; // rinse integral
         disabled = true;
+        motorL.disable();
+        motorR.disable();
     }
     else if (abs(angleX) < (float)50.0f && disabled)
     {
         disabled = false;
+        motorL.enable();
+        motorR.enable();
     }
 
     /* --------------------------
@@ -112,9 +119,9 @@ void controlFunc()
     // Integrating the output to obtain acceleration
     if (!disabled)
     {
-        controlOutput += angleControl.run(dT, angleX, angleSP);
+        controlOutput += angleControl.run(dT, forceX, angleSP);
 
-        controlOutput = constrain(controlOutput, 800.0f);
+        controlOutput = constrain(controlOutput, 3000.0f);
     }
     
     /* --------------------------
@@ -136,6 +143,18 @@ void controlFunc()
     }
 }
 
+void readSerialContext()
+{
+    while (true)
+    {
+        while (serialMotorOutputs.readable()) 
+        {
+            serialMotorOutputs.putc(serialMotorOutputs.getc());
+        }
+    }
+    Thread::wait(1000);
+}
+
 // This context just pulses the blue LED
 void pulseLedContext()
 {
@@ -155,11 +174,14 @@ void pulseLedContext()
     }
 }
 
-int i = 0;
-
 // main() runs in its own thread
 int main() {
 
+    serialMotorOutputs.baud(115200);
+    // Test thread for serial
+    Thread serialThread;
+    serialThread.start(callback(&readSerialContext));
+
     // MPU startup at 100hz
     if(imu.init(10,BITS_DLPF_CFG_188HZ)){
         printf("\nCouldn't initialize MPU6000 via SPI!");