Framkvæmdi test á hegðun PI_controller

docs/diagrams/src/motor_state.wsd

@@ -11,20 +11,26 @@ 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
     Operational : Exit / Stop motor and LED off
 
+state Stopped
+    Stopped : Entry / Stop motor
+    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
+
+Stopped --> Initialization : reset
+Stopped --> Operational : set operational
 
 @enduml

include/motor_controller.h

@@ -20,6 +20,7 @@ private:
 
     State& initialization_state;
     State& operational_state;
+    State& stopped_state;
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
@@ -36,7 +37,8 @@ public:
         MotorDriver& motor,
         P_controller& controller,
         State& initialization,
-        State& operational
+        State& operational,
+        State& stopped
     );
 
 
@@ -53,6 +55,10 @@ public:
 
     void to_operational();
 
+    void to_stopped();
+
+    bool fault_active();
+
 
     void run_speed_controller();
 
@@ -60,6 +66,8 @@ public:
 
     void reset_speed_measurement();
 
+    void reset_controller();
+
     void led_on();
 
     void led_off();

include/motor_driver.h

@@ -9,10 +9,14 @@ public:
     void begin();
 
     void forward();
+
     void reverse();
+
     void stop();
 
     void set(double pwm);
+
+    bool fault_active();   // Fault detection for part 2
 };
 
 #endif

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

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

include/stopped_state.h

@@ -0,0 +1,25 @@
+#ifndef STOPPED_STATE_H
+#define STOPPED_STATE_H
+
+#include "state.h"
+
+class StoppedState : public State
+{
+private:
+    unsigned long previous_time; // Used for blinking LED
+    bool led_state;              // Ditto
+
+public:
+    StoppedState();
+
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+
+};
+
+#endif

platformio.ini

@@ -14,4 +14,12 @@ board = nanoatmega328new
 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/main.cpp

@@ -1,29 +1,37 @@
+// 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"
 
 #include "initialization_state.h"
 #include "operational_state.h"
+#include "stopped_state.h"
 
 
 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;
 
 OperationalState operational_state;
 
+StoppedState stopped_state;
 
 // Stöðurnar verða að vera skilgreindar á undan motor_controller
 MotorController motor_controller(
@@ -31,7 +39,8 @@ MotorController motor_controller(
     motor,
     controller,
     initialization_state,
-    operational_state
+    operational_state,
+    stopped_state
 );
 
 
@@ -50,6 +59,12 @@ int main()
 
     sei();
     
+    // motor_controller.set_states(
+    //     &initialization_state,
+    //     &operational_state,
+    //     &stopped_state
+    // );
+
 
     motor_controller.begin();
 
@@ -77,3 +92,4 @@ int main()
 // int main(){
 //     return 0;
 // }
+#endif // PIO_UNIT_TESTING

src/motor_controller.cpp

@@ -9,11 +9,13 @@ MotorController::MotorController(
     MotorDriver& motor,
     P_controller& controller,
     State& initialization,
-    State& operational
+    State& operational,
+    State& stopped
 )
     : current_state(nullptr),
       initialization_state(initialization),
       operational_state(operational),
+      stopped_state(stopped),
       encoder(encoder),
       motor(motor),
       controller(controller),
@@ -22,9 +24,22 @@ MotorController::MotorController(
 {
     initialization_state.set_context(this);
     operational_state.set_context(this);
+    stopped_state.set_context(this);
 }
 
 
+// void MotorController::set_states(
+//     State* initialization,
+//     State* operational,
+//     State* stopped
+// )
+
+    // initialization_state = initialization;
+    // operational_state = operational;
+    // stopped_state = stopped;
+
+
+
 void MotorController::begin()
 {
     led.init();
@@ -105,14 +120,28 @@ void MotorController::reset_speed_measurement()
     encoder.reset_speed_measurement();
 }
 
+void MotorController::reset_controller()
+{
+    controller.reset();
+}
+
+void MotorController::to_stopped()
+{
+    transition_to(stopped_state);
+}
+
+bool MotorController::fault_active()
+{
+    return motor.fault_active();
+}
 
 void MotorController::led_on()
 {
-    led.set_hi();
+   led.set_hi();
 }
 
 
 void MotorController::led_off()
 {
-    led.set_lo();
+   led.set_lo();
 }

src/motor_driver.cpp

@@ -43,10 +43,16 @@ void MotorDriver::begin()
 {
     // Arduino D3  = PD3 = AIN1
     // Arduino D11 = PB3 = AIN2
+    // Arduino D4  = PD4 = FLT 
 
     DDRD |= (1 << DDD3);
     DDRB |= (1 << DDB3);
 
+    DDRD &= ~(1 << DDD4); 
+
+    PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
+
+
     // Timer2 Fast PWM
     TCCR2A = 0;
     TCCR2B = 0;
@@ -116,3 +122,9 @@ void MotorDriver::set(double pwm)
         ain2_pwm(duty);
     }
 }
+
+
+bool MotorDriver::fault_active()
+{
+    return !(PIND & (1 << PIND4));
+}

src/operational_state.cpp

@@ -8,12 +8,31 @@ void OperationalState::enter()
 {
     Serial.println("State: Operational");
 
+    context->reset_controller();
+
     context->led_on();
 }
 
+// Part 1 
+
+//void OperationalState::update()
+//{
+//    context->run_speed_controller();
+//}
+
+// Part 2
 
 void OperationalState::update()
 {
+    if (context->fault_active())
+    {
+        Serial.println("Fault detected");
+
+        context->to_stopped();
+
+        return;
+    }
+
     context->run_speed_controller();
 }
 

src/pi_controller.cpp

@@ -0,0 +1,32 @@
+#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;
+    // return 0.0;
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}

src/stopped_state.cpp

@@ -0,0 +1,70 @@
+#include "stopped_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+StoppedState::StoppedState()
+    : previous_time(0),
+      led_state(false)
+{
+}
+
+
+void StoppedState::enter()
+{
+    Serial.println("State: Stopped");
+
+    context->stop_motor();
+
+    led_state = false;
+
+    digitalWrite(LED_BUILTIN, LOW);
+
+    previous_time = millis();
+}
+
+// Controlling the blinking of the LED @ 2Hz
+
+void StoppedState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 250)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        digitalWrite(
+            LED_BUILTIN,
+            led_state ? HIGH : LOW
+        );
+    }
+}
+
+
+// Commands for Stopped [ r = reset and o for operational]
+
+void StoppedState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+}
+
+
+void StoppedState::exit()
+{
+    digitalWrite(LED_BUILTIN, LOW);
+}

test/test_pi_controller/test_main.cpp

@@ -0,0 +1,150 @@
+#include <unity.h>
+
+// #ifdef ARDUINO
+#include <Arduino.h>
+// #endif
+
+#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-4;  // double er 32-bita float a AVR
+
+// 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_FLOAT_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_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_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