Bætti við test-um, pi controller með polymorphism og bætti við virkni í operational mode

include/motor_controller.h

@@ -66,6 +66,8 @@ public:
 
     void reset_speed_measurement();
 
+    void reset_controller();
+
     void led_on();
 
     void led_off();

include/p_controller.h

@@ -3,13 +3,14 @@
 
 class P_controller
 {
-private:
+protected:
     double Kp;
 
 public:
     P_controller(double Kp);
-
-    double update(double ref, double actual);
+    virtual ~P_controller() {}
+    virtual double update(double ref, double actual);
+    virtual void reset() {}
 };
 
-#endif
+#endif

include/pi_controller.h

@@ -0,0 +1,21 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+#include "p_controller.h"
+
+class PI_controller : public P_controller
+{
+    private:
+        static constexpr double U_MAX = 100.0;  // %
+        static constexpr double U_MIN = -100.0; // %
+
+        double Ti;
+        double T;
+        double integral;
+    public:
+        PI_controller(double Kp, double Ti, double T);
+        double update(double ref, double actual) override;
+        void reset() override;
+};
+
+#endif

platformio.ini

@@ -15,3 +15,9 @@ framework = arduino
 
 monitor_speed = 115200
 
+
+[env:native]
+platform = native
+test_framework = unity
+test_build_src = yes
+build_src_filter = -<*> +<p_controller.cpp> +<pi_controller.cpp>

src/main.cpp

@@ -3,7 +3,7 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-#include "p_controller.h"
+#include "pi_controller.h"
 
 #include "motor_controller.h"
 
@@ -16,9 +16,12 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-const double KP = 12.5;
+const double Kp = 12.5;
+const double Ti = 1.0;
+const double T = 0.004;
 
-P_controller controller(KP);
+// P_controller controller(Kp);
+PI_controller controller(Kp, Ti, T);
 
 
 InitializationState initialization_state;

src/motor_controller.cpp

@@ -120,6 +120,11 @@ void MotorController::reset_speed_measurement()
     encoder.reset_speed_measurement();
 }
 
+void MotorController::reset_controller()
+{
+    controller.reset();
+}
+
 void MotorController::to_stopped()
 {
     transition_to(stopped_state);

src/operational_state.cpp

@@ -8,6 +8,8 @@ void OperationalState::enter()
 {
     Serial.println("State: Operational");
 
+    context->reset_controller();
+
     context->led_on();
 }
 

src/pi_controller.cpp

@@ -0,0 +1,31 @@
+#include "pi_controller.h"
+
+PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), T(T), integral(0.0)
+{
+}
+
+double PI_controller::update(double ref, double actual)
+{
+    double e = ref - actual;
+    double new_integral = integral + e * T;
+    double u = Kp * (e + new_integral / Ti);
+
+    if (u > U_MAX)
+    {
+        u = U_MAX;
+    }
+    else if (u < U_MIN)
+    {
+        u = U_MIN;
+    }
+    else
+    {
+        integral = new_integral;
+    }
+    return u;
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}

test/test_pi_controller/test_main.cpp

@@ -0,0 +1,124 @@
+#include <unity.h>
+
+#include "p_controller.h"
+#include "pi_controller.h"
+
+static const double KP = 2.0;
+static const double TI = 1.0;
+static const double T = 0.1;
+static const double TOL = 1e-6;
+
+void setUp(void) {}
+void tearDown(void) {}
+
+// Normal flow: engin villa -> ekkert úttak
+void test_zero_error_gives_zero_output(void)
+{
+    PI_controller c(KP, TI, T);
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, 0.0, c.update(10.0, 10.0));
+}
+
+// Normal flow: fyrsta skref u = Kp * (e + e*T/Ti)
+void test_first_step(void)
+{
+    PI_controller c(KP, TI, T);
+    double e = 5.0;
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0));
+}
+
+// Normal flow: stöðug villa safnast upp, u_n = Kp * (e + n*e*T/Ti)
+void test_integral_accumulates(void)
+{
+    PI_controller c(KP, TI, T);
+    double e = 5.0;
+    double u = 0.0;
+    for (int n = 1; n <= 5; n++)
+    {
+        u = c.update(e, 0.0);
+        TEST_ASSERT_DOUBLE_WITHIN(TOL, KP * (e + n * e * T / TI), u);
+    }
+}
+
+// Normal flow: þegar villan er orðin 0 heldur integralið úttakinu uppi
+void test_output_held_when_error_zero(void)
+{
+    PI_controller c(KP, TI, T);
+    double e = 5.0;
+    for (int n = 0; n < 5; n++)
+    {
+        c.update(e, 0.0);
+    }
+    double E = 5 * e * T;
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, KP * E / TI, c.update(0.0, 0.0));
+}
+
+// Alt flow: villa langt utan getu -> mettun í +100 %
+void test_saturates_high(void)
+{
+    PI_controller c(KP, TI, T);
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, 100.0, c.update(10000.0, 0.0));
+}
+
+// Alt flow: mettun í -100 %
+void test_saturates_low(void)
+{
+    PI_controller c(KP, TI, T);
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, -100.0, c.update(-10000.0, 0.0));
+}
+
+// Alt flow: anti-windup. Eftir langa mettun á lítil neikvæð villa
+// strax að gefa úttak undir mettun (integralið óx ekki)
+void test_no_integral_windup(void)
+{
+    PI_controller c(KP, TI, T);
+    for (int n = 0; n < 1000; n++)
+    {
+        c.update(10000.0, 0.0);
+    }
+    double u = c.update(0.0, 1.0);  // e = -1
+    TEST_ASSERT_TRUE(u < 0.0);
+}
+
+// Alt flow: reset (stopped -> operational) hreinsar integralið
+void test_reset_clears_integral(void)
+{
+    PI_controller c(KP, TI, T);
+    for (int n = 0; n < 5; n++)
+    {
+        c.update(5.0, 0.0);
+    }
+    c.reset();
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, 0.0, c.update(0.0, 0.0));
+}
+
+// Polymorphism: kall í gegnum P_controller& gefur PI hegðun
+void test_polymorphic_update(void)
+{
+    PI_controller pi(KP, TI, T);
+    P_controller& c = pi;
+    double e = 5.0;
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0));
+}
+
+// P_controller hegðar sér óbreytt
+void test_p_controller_unchanged(void)
+{
+    P_controller c(KP);
+    TEST_ASSERT_DOUBLE_WITHIN(TOL, KP * 5.0, c.update(5.0, 0.0));
+}
+
+int main(int argc, char** argv)
+{
+    UNITY_BEGIN();
+    RUN_TEST(test_zero_error_gives_zero_output);
+    RUN_TEST(test_first_step);
+    RUN_TEST(test_integral_accumulates);
+    RUN_TEST(test_output_held_when_error_zero);
+    RUN_TEST(test_saturates_high);
+    RUN_TEST(test_saturates_low);
+    RUN_TEST(test_no_integral_windup);
+    RUN_TEST(test_reset_clears_integral);
+    RUN_TEST(test_polymorphic_update);
+    RUN_TEST(test_p_controller_unchanged);
+    return UNITY_END();
+}