From 11df24f592a7be73ce66d74e93483cbb183944d2 Mon Sep 17 00:00:00 2001 From: helgie22 Date: Thu, 1 Oct 2026 09:47:04 +0000 Subject: [PATCH] =?UTF-8?q?B=C3=A6tti=20vi=C3=B0=20test-um,=20pi=20control?= =?UTF-8?q?ler=20me=C3=B0=20polymorphism=20og=20b=C3=A6tti=20vi=C3=B0=20vi?= =?UTF-8?q?rkni=20=C3=AD=20operational=20mode?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/motor_controller.h | 2 + include/p_controller.h | 9 +- include/pi_controller.h | 21 +++++ platformio.ini | 6 ++ src/main.cpp | 9 +- src/motor_controller.cpp | 5 ++ src/operational_state.cpp | 2 + src/pi_controller.cpp | 31 +++++++ test/test_pi_controller/test_main.cpp | 124 ++++++++++++++++++++++++++ 9 files changed, 202 insertions(+), 7 deletions(-) create mode 100644 include/pi_controller.h create mode 100644 src/pi_controller.cpp create mode 100644 test/test_pi_controller/test_main.cpp diff --git a/include/motor_controller.h b/include/motor_controller.h index d8cba89..02dd9dc 100644 --- a/include/motor_controller.h +++ b/include/motor_controller.h @@ -66,6 +66,8 @@ public: void reset_speed_measurement(); + void reset_controller(); + void led_on(); void led_off(); diff --git a/include/p_controller.h b/include/p_controller.h index 2a6a802..82d2958 100644 --- a/include/p_controller.h +++ b/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 \ No newline at end of file +#endif diff --git a/include/pi_controller.h b/include/pi_controller.h new file mode 100644 index 0000000..341e116 --- /dev/null +++ b/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 diff --git a/platformio.ini b/platformio.ini index be95bc9..80a4969 100644 --- a/platformio.ini +++ b/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 = -<*> + + diff --git a/src/main.cpp b/src/main.cpp index 9a118e4..b1acc31 100644 --- a/src/main.cpp +++ b/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; diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index 3efb811..5d72809 100644 --- a/src/motor_controller.cpp +++ b/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); diff --git a/src/operational_state.cpp b/src/operational_state.cpp index 2f07ff3..b081512 100644 --- a/src/operational_state.cpp +++ b/src/operational_state.cpp @@ -8,6 +8,8 @@ void OperationalState::enter() { Serial.println("State: Operational"); + context->reset_controller(); + context->led_on(); } diff --git a/src/pi_controller.cpp b/src/pi_controller.cpp new file mode 100644 index 0000000..f5f3846 --- /dev/null +++ b/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; +} diff --git a/test/test_pi_controller/test_main.cpp b/test/test_pi_controller/test_main.cpp new file mode 100644 index 0000000..10c4732 --- /dev/null +++ b/test/test_pi_controller/test_main.cpp @@ -0,0 +1,124 @@ +#include + +#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(); +}