diff --git a/CMakeLists.txt b/CMakeLists.txt
index c76b130..1bc272d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,11 +11,6 @@ enable_testing()
 
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
 
-## Add C++ executables here
-  add_executable(pi_test tests/pi_test.cpp)
-  target_link_libraries(pi_test ${PROJECT_NAME})
-  add_test(NAME pi_test COMMAND "${CMAKE_SOURCE_DIR}/bin/pi_test")
-
 # Declare a C++ library
 add_library(bemf_observer
     src/controllers/pid_controller.cpp
@@ -27,6 +22,15 @@ add_library(bemf_observer
     src/observers/dq_update.cpp
 )
 
+## Add C++ tests here
+add_executable(pi_test tests/pi_test.cpp)
+target_link_libraries(pi_test ${PROJECT_NAME})
+add_test(NAME pi_test COMMAND "${CMAKE_SOURCE_DIR}/bin/pi_test")
+
+add_executable(pid_test tests/pid_test.cpp)
+target_link_libraries(pid_test ${PROJECT_NAME})
+add_test(NAME pid_test COMMAND "${CMAKE_SOURCE_DIR}/bin/pid_test")
+
 # Include Header files
 include_directories(
     include
diff --git a/include/math/functions.hpp b/include/math/functions.hpp
index b24f9eb..e2681f5 100644
--- a/include/math/functions.hpp
+++ b/include/math/functions.hpp
@@ -4,6 +4,7 @@
 
 namespace math
 {
+float abs(float input);
 float pow(float input, int power);
 float wrap_angle(float input);
 float sin(float angle);
diff --git a/src/math/functions.cpp b/src/math/functions.cpp
index 6e9cfb5..dce8629 100644
--- a/src/math/functions.cpp
+++ b/src/math/functions.cpp
@@ -1,5 +1,13 @@
 #include <math/functions.hpp>
 
+float math::abs(float input)
+{
+  if (input < 0)
+    return -input;
+  else
+    return input;
+}
+
 float math::pow(float input, int power)
 {
   float output = 1;
diff --git a/tests/pi_test.cpp b/tests/pi_test.cpp
index 5534d2e..09681fa 100644
--- a/tests/pi_test.cpp
+++ b/tests/pi_test.cpp
@@ -1,5 +1,6 @@
 #include <iostream>
 #include <controllers/pi_controller.hpp>
+#include <math/functions.hpp>
 #define Ts 0.01
 
 using namespace controllers;
@@ -12,7 +13,10 @@ int main()
   std::cout << " Starting loop" << std::endl;
   for (float t = 0; t < 10; t += Ts)
   {
-    x_meas += Ts * controller.loop(x_ref - x_meas, config);
+    float error = x_ref - x_meas;
+    if(math::abs(error) < 0.1 && t>5)
+      break;
+    x_meas += Ts * controller.loop(error, config);
     std::cout << "t = " << t << " x = " << x_meas << std::endl;
   }
 }
diff --git a/tests/pid_test.cpp b/tests/pid_test.cpp
new file mode 100644
index 0000000..a760f2d
--- /dev/null
+++ b/tests/pid_test.cpp
@@ -0,0 +1,22 @@
+#include <iostream>
+#include <controllers/pid_controller.hpp>
+#include <math/functions.hpp>
+#define Ts 0.01
+
+using namespace controllers;
+
+int main()
+{
+  float x_meas = 0, x_ref = 10;
+  pid_config config = { 2, 1, 0.01, Ts, 10, -10 };
+  pid_controller controller;
+  std::cout << " Starting loop" << std::endl;
+  for (float t = 0; t < 10; t += Ts)
+  {
+    float error = x_ref - x_meas;
+    if(math::abs(error) < 0.1 && t>5)
+      break;
+    x_meas += Ts * controller.loop(error, config);
+    std::cout << "t = " << t << " x = " << x_meas << std::endl;
+  }
+}