diff --git a/include/math/cordic.hpp b/include/math/cordic.hpp
index 683dbca..0ffb2ca 100644
--- a/include/math/cordic.hpp
+++ b/include/math/cordic.hpp
@@ -6,8 +6,7 @@ namespace CORDIC
 {
 struct Trig
 {
-  int sin;
-  int cos;
+  int cos, sin;
 };
-Trig trig(int angle);
+Trig trig(int32_t angle);
 }  // namespace CORDIC
diff --git a/src/math/cordic.cpp b/src/math/cordic.cpp
index d9948d9..9707f56 100644
--- a/src/math/cordic.cpp
+++ b/src/math/cordic.cpp
@@ -1,18 +1,28 @@
 #include <math/cordic.hpp>
 #include <iostream>
+#include <iostream>
 
-CORDIC::Trig CORDIC::trig(int angle)
+CORDIC::Trig CORDIC::trig(int32_t angle)
 {
+  int8_t quadAdj = 1; 
+  //Shift angle to be in range -90 to 90
+  if (angle > 16384) {
+    angle = angle - 32768;
+    quadAdj = -1;
+  } 
+  else if (angle < -16384) {
+    angle = angle + 32768;
+    quadAdj = -1;
+  }
   int32_t atan2[14] = { 8192, 4836, 2555, 1297, 651, 325, 162, 81, 40, 20, 10, 5, 2, 1 };
-  int32_t x = 9949, y = 0, theta = 0, sigma, nx;
+  int32_t x = 9949, y = 0, theta = 0;
   for (int32_t i = 0; i < 14; i++)
   {
-    sigma = (theta < angle) ? 1 : -1;
+    int8_t sigma = (angle > theta) ? 1 : -1;
     theta = theta + sigma * atan2[i];
-    if (theta == angle) break;
-    nx = x - sigma * (y >> i);
+    int32_t nx = x - sigma * (y >> i);
     y = y + sigma * (x >> i);
     x = nx;
   }
-  return { y, x };
+  return { quadAdj*x, quadAdj*y };
 }
diff --git a/tests/cordic_algo_test.cpp b/tests/cordic_algo_test.cpp
index b3745ab..403f648 100644
--- a/tests/cordic_algo_test.cpp
+++ b/tests/cordic_algo_test.cpp
@@ -3,23 +3,18 @@
 #include <math/functions.hpp>
 #include <math/cordic.hpp>
 
-int main(int argc, char** argv)
+int main()
 {
-  if (argc != 2)
+  for(int i = -180; i <= 180; i++)
   {
-    std::cerr << "Usage: " << argv[0] << " <angle>" << std::endl;
-    return 1;
+    int angle_cordic = (i << 16) / 360;
+    CORDIC::Trig trig = CORDIC::trig(angle_cordic);
+    float sine = (float)trig.sin / 16384.0F;
+    float cosine = (float)trig.cos / 16384.0F;
+    if(math::abs(math::sin(i*PI/180) - sine) > 0.001 || math::abs(math::cos(i*PI/180) - cosine) > 0.001) {
+      std::cerr << "Error exceeded threshold at angle: " << i << std::endl;
+      return 1;
+    }
   }
-  int angle = atoi(argv[1]);
-  int angle_cordic = (angle << 16) / 360;
-  CORDIC::Trig trig = CORDIC::trig(angle_cordic);
-  std::cout << "sin(x) in cru = " << trig.sin << " "
-            << "cos(x) in cru = " << trig.cos << std::endl;
-  float sine = (float)trig.sin / 16384.0F;
-  float cosine = (float)trig.cos / 16384.0F;
-  std::cout << "std::sin(x) error = " << std::sin(angle * PI / 180) - sine << " "
-            << "std::cos(x) error = " << std::cos(angle * PI / 180) - cosine << std::endl;
-  std::cout << "math::sin(x) error = " << math::sin(angle * PI / 180) - sine << " "
-            << "math::cos(x) error = " << math::cos(angle * PI / 180) - cosine << std::endl;
   return 0;
 }
diff --git a/tests/pi_test.cpp b/tests/pi_test.cpp
index ae438e6..7904fe1 100644
--- a/tests/pi_test.cpp
+++ b/tests/pi_test.cpp
@@ -14,9 +14,14 @@ int main()
   for (float t = 0; t < 10; t += Ts)
   {
     float error = x_ref - x_meas;
-    if (math::abs(error) < 0.1 && t > 5)
+    if (math::abs(error) < 0.01 && t > 5)
       break;
     x_meas += Ts * controller.loop(error, config);
     std::cout << "t = " << t << " x = " << x_meas << std::endl;
   }
+  if(math::abs(x_meas - x_ref) > 0.01) {
+    std::cerr << "Steady state error exceeded" << std::endl;
+    return 1;
+  }
+  return 0;
 }