diff --git a/platformio.ini b/platformio.ini
index be95bc9..2425b33 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -14,4 +14,5 @@ board = nanoatmega328new
 framework = arduino
 
 monitor_speed = 115200
-
+test_framework = unity
+test_build_src = yes
diff --git a/src/main.cpp b/src/main.cpp
index e43abc6..fad6984 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,7 +25,7 @@ PI_controller controller(
     12.5,           // Kp
     1.0,            // Ti
     0.004           // T = 4 ms
-)
+);
 
 InitializationState initialization_state;
 
diff --git a/src/pi_controller.cpp b/src/pi_controller.cpp
index e78ebe3..92f52cc 100644
--- a/src/pi_controller.cpp
+++ b/src/pi_controller.cpp
@@ -13,39 +13,43 @@ PI_controller::PI_controller(
 {
 }
 
-
 double PI_controller::update(double ref, double actual)
 {
-    double error = ref - actual;
-
-    double new_integral =
-        integral + error * T;
-
-    double output =
-        Kp * (error + new_integral / Ti);
-
-
-    // Positive saturation
-    if (output > 100.0)
-    {
-        return 100.0;
-    }
-
-
-    // Negative saturation
-    if (output < -100.0)
-    {
-        return -100.0;
-    }
-
-
-    // Only save the integral if the controller
-    // is not saturated
-    integral = new_integral;
-
-    return output;
+    return 0.0;
 }
 
+// double PI_controller::update(double ref, double actual)
+// {
+//     double error = ref - actual;
+
+//     double new_integral =
+//         integral + error * T;
+
+//     double output =
+//         Kp * (error + new_integral / Ti);
+
+
+//     // Positive saturation
+//     if (output > 100.0)
+//     {
+//         return 100.0;
+//     }
+
+
+//     // Negative saturation
+//     if (output < -100.0)
+//     {
+//         return -100.0;
+//     }
+
+
+//     // Only save the integral if the controller
+//     // is not saturated
+//     integral = new_integral;
+
+//     return output;
+// }
+
 
 void PI_controller::reset()
 {
diff --git a/test/test_pi_controller/test_main.cpp b/test/test_pi_controller/test_main.cpp
new file mode 100644
index 0000000..4cb1400
--- /dev/null
+++ b/test/test_pi_controller/test_main.cpp
@@ -0,0 +1,28 @@
+#include <unity.h>
+
+//#include "p_controller.h"
+#include "pi_controller.h"
+
+static const double KP = 2.0;
+static const double KI = 1.0;
+static const double T = 0.1;
+static const double TOL = 1e-4; // Tolerance
+
+void test_positive_error()
+{
+    PI_controller controller(KP, KI, T);
+
+    double output = controller.update(10.0, 8.0);
+
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, 4.4, output);
+}
+
+
+int main()
+{
+    UNITY_BEGIN();
+
+    RUN_TEST(test_positive_error);
+
+    return UNITY_END();
+}
\ No newline at end of file