From 36fce02ff21cad0f93ccac8d3dcf790c611bf233 Mon Sep 17 00:00:00 2001
From: isakg22 <isakg22@ru.is>
Date: Thu, 1 Oct 2026 11:13:28 +0000
Subject: [PATCH] =?UTF-8?q?Byrja=C3=B0i=20=C3=A1=20k=C3=B3=C3=B0um=20fyrir?=
 =?UTF-8?q?=20part=203=20en=20lenti=20=C3=AD=20veseni=20me=C3=B0=20test=20?=
 =?UTF-8?q?k=C3=B3=C3=B0ann?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 platformio.ini                        |  3 +-
 src/main.cpp                          |  2 +-
 src/pi_controller.cpp                 | 62 ++++++++++++++-------------
 test/test_pi_controller/test_main.cpp | 28 ++++++++++++
 4 files changed, 64 insertions(+), 31 deletions(-)
 create mode 100644 test/test_pi_controller/test_main.cpp

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