Byrjaði á kóðum fyrir part 3 en lenti í veseni með test kóðann

platformio.ini

@@ -14,4 +14,5 @@ board = nanoatmega328new
 framework = arduino
 
 monitor_speed = 115200
-
+test_framework = unity
+test_build_src = yes

src/main.cpp

@@ -25,7 +25,7 @@ PI_controller controller(
     12.5,           // Kp
     1.0,            // Ti
     0.004           // T = 4 ms
-)
+);
 
 InitializationState initialization_state;
 

src/pi_controller.cpp

@@ -13,38 +13,42 @@ 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;
+    return 0.0;
 }
 
+// double PI_controller::update(double ref, double actual)
+// {
+//     double error = ref - actual;
 
-    // Negative saturation
-    if (output < -100.0)
-    {
-        return -100.0;
-    }
+//     double new_integral =
+//         integral + error * T;
+
+//     double output =
+//         Kp * (error + new_integral / Ti);
 
 
-    // Only save the integral if the controller
-    // is not saturated
-    integral = new_integral;
+//     // Positive saturation
+//     if (output > 100.0)
+//     {
+//         return 100.0;
+//     }
 
-    return output;
-}
+
+//     // 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()

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();
+}