Framkvæmdi test á hegðun PI_controller

docs/diagrams/src/motor_state.wsd

@@ -11,13 +11,11 @@ skinparam state {
 
 state Initialization 
     Initialization : Entry / Stop motor 
-    Initialization : Do / Initialize internal parameters
-    Initialization : Do / Reset speed measurement
+    Initialization : Entry / Initialize internal parameters
+    Initialization : Entry / Reset speed measurement
     Initialization : Do / Wait 2 seconds
     Initialization : Exit / Send Boot-up message
 
-
-
 state Operational 
     Operational : Entry / LED on
     Operational : Do / Run speed controller
@@ -28,9 +26,7 @@ state Stopped
     Stopped : Do / Blink LED at 2 Hz
     Stopped : Exit / LED off
 
-
-
-Initialization --> Operational : initialization\n complete
+Initialization --> Operational : initialization complete
 Operational --> Initialization : reset
 Operational --> Stopped : fault detected
 

include/controller.h

@@ -1,18 +0,0 @@
-#ifndef CONTROLLER_H
-#define CONTROLLER_H
-
-class Controller
-{
-public:
-    virtual ~Controller()
-    {
-    }
-
-    virtual double update(double ref, double actual) = 0;
-
-    virtual void reset()
-    {
-    }
-};
-
-#endif

include/initialization_state.h

@@ -7,7 +7,7 @@
 class InitializationState : public State
 {
 private:
-    static const unsigned long WAIT_MS = 2000UL;
+    static const unsigned long WAIT_MS = 2000;
 
     unsigned long enter_time_ms = 0;
 

include/motor_controller.h

@@ -3,9 +3,7 @@
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-//#include "p_controller.h"
-//#include "pi_controller.h"
-#include "controller.h"
+#include "p_controller.h"
 #include "digital_out.h"
 
 class State;
@@ -26,9 +24,7 @@ private:
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
-    //P_controller& controller;
-    //PI_controller& controller;
-    Controller& controller;
+    P_controller& controller;
 
     Digital_out led;
 
@@ -39,21 +35,13 @@ public:
     MotorController(
         RotaryEncoderDriver& encoder,
         MotorDriver& motor,
-        //P_controller& controller,
-        Controller& controller,
+        P_controller& controller,
         State& initialization,
         State& operational,
         State& stopped
     );
 
 
-    // void set_states(
-    //     State* initialization,
-    //     State* operational,
-    //     State* stopped
-    // );
-
-
     void begin();
 
     void update();
@@ -80,15 +68,9 @@ public:
 
     void reset_controller();
 
-
-
     void led_on();
 
     void led_off();
-
-    
 };
 
-
-
 #endif

include/p_controller.h

@@ -1,20 +1,16 @@
 #ifndef P_CONTROLLER_H
 #define P_CONTROLLER_H
 
-#include "controller.h"
-
-class P_controller : public Controller
+class P_controller
 {
-private:
+protected:
     double Kp;
 
 public:
     P_controller(double Kp);
-
-    double update(double ref, double actual) override;
-
-
-    
+    virtual ~P_controller() {}
+    virtual double update(double ref, double actual);
+    virtual void reset() {}
 };
 
 #endif

include/pi_controller.h

@@ -1,27 +1,21 @@
 #ifndef PI_CONTROLLER_H
 #define PI_CONTROLLER_H
 
-#include "controller.h"
+#include "p_controller.h"
 
-class PI_controller : public Controller
+class PI_controller : public P_controller
 {
-private:
-    double Kp;
-    double Ti;
-    double T;
+    private:
+        static constexpr double U_MAX = 100.0;  // %
+        static constexpr double U_MIN = -100.0; // %
 
-    double integral;
-
-public:
-    PI_controller(
-        double Kp,
-        double Ti,
-        double T
-    );
-
-    double update(double ref, double actual) override;
-
-    void reset() override;
+        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

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

src/initialization_state.cpp

@@ -15,10 +15,7 @@ void InitializationState::enter()
 
     context->reset_speed_measurement();
 
-    context->reset_controller();
-
     enter_time_ms = millis();
-
 }
 
 

src/main.cpp

@@ -1,9 +1,11 @@
+// main.cpp er sleppt i test-build (pio test) svo setup()/loop() rekist ekki a prof
+#ifndef PIO_UNIT_TESTING
+
 #include <Arduino.h>
 #include <avr/interrupt.h>
 
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
-//#include "p_controller.h"
 #include "pi_controller.h"
 
 #include "motor_controller.h"
@@ -17,15 +19,13 @@ 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);
 
-PI_controller controller(
-    12.5,           // Kp
-    1.0,            // Ti
-    0.004           // T = 4 ms
-);
 
 InitializationState initialization_state;
 
@@ -59,7 +59,6 @@ int main()
 
     sei();
     
-
     // motor_controller.set_states(
     //     &initialization_state,
     //     &operational_state,
@@ -93,3 +92,4 @@ int main()
 // int main(){
 //     return 0;
 // }
+#endif // PIO_UNIT_TESTING

src/motor_controller.cpp

@@ -7,9 +7,7 @@
 MotorController::MotorController(
     RotaryEncoderDriver& encoder,
     MotorDriver& motor,
-    //P_controller& controller,
-    //PI_controller& controller;
-    Controller& controller,
+    P_controller& controller,
     State& initialization,
     State& operational,
     State& stopped
@@ -60,8 +58,6 @@ void MotorController::transition_to(State& new_state)
 
     current_state = &new_state;
 
-    current_state->set_context(this);
-
     current_state->enter();
 }
 
@@ -124,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);
@@ -134,11 +135,6 @@ bool MotorController::fault_active()
     return motor.fault_active();
 }
 
-void MotorController::reset_controller()
-{
-    controller.reset();
-}
-
 void MotorController::led_on()
 {
    led.set_hi();
@@ -149,4 +145,3 @@ void MotorController::led_off()
 {
    led.set_lo();
 }
-

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

@@ -1,56 +1,31 @@
 #include "pi_controller.h"
 
-
-PI_controller::PI_controller(
-    double Kp,
-    double Ti,
-    double T
-)
-    : Kp(Kp),
-      Ti(Ti),
-      T(T),
-      integral(0.0)
+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)
 {
-    return 0.0;
+    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;
+    // 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()
 {
     integral = 0.0;

test/test_pi_controller/test_main.cpp

@@ -1,28 +1,150 @@
 #include <unity.h>
 
-//#include "p_controller.h"
+// #ifdef ARDUINO
+#include <Arduino.h>
+// #endif
+
+#include "p_controller.h"
 #include "pi_controller.h"
 
 static const double KP = 2.0;
-static const double KI = 1.0;
+static const double TI = 1.0;
 static const double T = 0.1;
-static const double TOL = 1e-4; // Tolerance
+static const double TOL = 1e-4;  // double er 32-bita float a AVR
 
-void test_positive_error()
+// void setUp(void) {}
+// void tearDown(void) {}
+
+// Normal flow: engin villa -> ekkert úttak
+void test_zero_error_gives_zero_output(void)
 {
-    PI_controller controller(KP, KI, T);
-
-    double output = controller.update(10.0, 8.0);
-
-    TEST_ASSERT_DOUBLE_WITHIN(TOL, 4.4, output);
+    PI_controller c(KP, TI, T);
+    TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(10.0, 10.0));
 }
 
-
-int main()
+// 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_FLOAT_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_FLOAT_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_FLOAT_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_FLOAT_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_FLOAT_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_FLOAT_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_FLOAT_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_FLOAT_WITHIN(TOL, KP * 5.0, c.update(5.0, 0.0));
+}
+
+void setup()
+{
+    // init();
+    delay(2000);
     UNITY_BEGIN();
-
-    RUN_TEST(test_positive_error);
-
-    return UNITY_END();
+    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();
+    UNITY_END();
 }
+
+void loop() {}
+
+// #ifdef ARDUINO
+// void setup()
+// {
+//     delay(2000);  // bida eftir serial tengingu eftir reset
+//     run_tests();
+// }
+
+// void loop()
+// {
+// }
+// #else
+// int main(int argc, char** argv)
+// {
+//     return run_tests();
+// }
+// #endif