From cfc419d708a9f76993db5dda099e8613ee873020 Mon Sep 17 00:00:00 2001 From: vSebas Date: Tue, 28 Jan 2025 23:57:24 -0600 Subject: [PATCH] added uts --- .../control_laws/pid_first_order_test.cpp | 31 +++++--- .../feedback_linearization/fblin_pid_test.cpp | 76 +++++++++++++++++++ .../feedback_linearization/fblin_test.cpp | 51 +++++++++++++ .../vtec_sdc1_pid_test.cpp | 51 +++++++++++++ 4 files changed, 199 insertions(+), 10 deletions(-) create mode 100644 tests/controllers/feedback_linearization/fblin_pid_test.cpp create mode 100644 tests/controllers/feedback_linearization/fblin_test.cpp create mode 100644 tests/controllers/feedback_linearization/vtec_sdc1_pid_test.cpp diff --git a/tests/controllers/control_laws/pid_first_order_test.cpp b/tests/controllers/control_laws/pid_first_order_test.cpp index 5d47d18..ef135aa 100644 --- a/tests/controllers/control_laws/pid_first_order_test.cpp +++ b/tests/controllers/control_laws/pid_first_order_test.cpp @@ -11,30 +11,32 @@ TEST(PID, SimpleModel){ double position = 0; double velocity = 0; + double expected_position = 5; + double precision = 0.1; // Run a simple simulation, check that setpoint is reached with full PID impl. for(int i = 0; i < 1000; i++){ - double u = pid.update(position, 5); + double u = pid.update(position, expected_position); position += (velocity + u) * param.kDt; velocity += 0.1 * param.kDt; } // Position should be near setpoint. - EXPECT_NEAR(position, 5, 0.1); + EXPECT_NEAR(position, expected_position, precision); pid = PID(param); position = 0; velocity = 0; // Run a simple simulation, check that setpoint is reached with full PID impl. for(int i = 0; i < 1000; i++){ - double u = pid.update(position, -5); + double u = pid.update(position, -expected_position); position += (velocity + u) * param.kDt; velocity += 0.1 * param.kDt; } - EXPECT_NEAR(position, -5, 0.1); + EXPECT_NEAR(position, -expected_position, precision); } TEST(PID, ClampU) { @@ -44,14 +46,17 @@ TEST(PID, ClampU) { param.kUMin = -100; param.enable_ramp_rate_limit = false; + double vel = 0; + double vel_d = 1; + PID pid(param); - EXPECT_LE(pid.updateSaturated(0, 1), 100); - EXPECT_GE(pid.updateSaturated(0, -1), -100); + EXPECT_LE(pid.updateSaturated(vel, vel_d), 100); + EXPECT_GE(pid.updateSaturated(vel, -vel_d), -100); PIDParameters no_negative_param = param; no_negative_param.kUMin = 0; PID no_negative_pid(no_negative_param); - EXPECT_GE(no_negative_pid.updateSaturated(0, -1), 0); + EXPECT_GE(no_negative_pid.updateSaturated(vel, -vel_d), 0); } TEST(PID, RampRateLimit){ @@ -61,12 +66,18 @@ TEST(PID, RampRateLimit){ param.ramp_rate = 1; param.kDt = 0.1; - PID pid(param); + double velocity = 0; + double velocity_d = 10; + double rate = 0.1; + double precision = 0.01; + PID pid(param); // U should only be allowed to rise by 0.1 - EXPECT_NEAR(pid.update(0, 10), 0.1, 0.01); + EXPECT_NEAR(pid.update(velocity, velocity_d), rate, precision); pid = PID(param); + velocity_d = 0.05; + rate = 0.05; // U should rise to any value less than 0.1 - EXPECT_NEAR(pid.update(0, 0.05), 0.05, 0.01); + EXPECT_NEAR(pid.update(velocity, velocity_d), rate, precision); } \ No newline at end of file diff --git a/tests/controllers/feedback_linearization/fblin_pid_test.cpp b/tests/controllers/feedback_linearization/fblin_pid_test.cpp new file mode 100644 index 0000000..0ec15f2 --- /dev/null +++ b/tests/controllers/feedback_linearization/fblin_pid_test.cpp @@ -0,0 +1,76 @@ +#include +#include "controllers/feedback_linearization/PID/first_order/fblin_pid.hpp" + +TEST(PIDLin, SimpleModel){ + + PIDParameters params; + params.kP = 10; + params.kI = 10; + params.kD = 0.1; + params.kDt = 0.01; + + double FB_LIN_UMAX = 255; + double FB_LIN_UMIN = -255; + double g_x = 1; + double accel_d = 1; + + double position = 0; + double velocity = 0; + + double position_d = 5; + double distance = 0.1; + + PIDLin pid(FB_LIN_UMAX, FB_LIN_UMIN, params); + + pid.g_x_ = g_x; + + // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux) + // Run a simple simulation, check that setpoint is reached with full PID impl. + for(int i = 0; i < 100; i++){ + double u = pid.calculateManipulations(position, position_d, accel_d); + position += (velocity + u) * params.kDt; + velocity += 0.1 * params.kDt; + } + + // Position should be near setpoint. + EXPECT_NEAR(position, position_d, distance); + + pid = PIDLin(FB_LIN_UMAX, FB_LIN_UMIN, params); + position = 0; + velocity = 0; + + // Run a simple simulation, check that setpoint is reached with full PID impl. + for(int i = 0; i < 1000; i++){ + double u = pid.calculateManipulations(position, -position_d, accel_d); + position -= (velocity + u) * params.kDt; + velocity += 0.1 * params.kDt; + } + + EXPECT_NEAR(position, -position_d, distance); +} + +TEST(PIDLin, ClampU) { + + PIDParameters params; + params.kP = 100; + params.kDt = 0.01; + + double FB_LIN_UMAX = 10; + double FB_LIN_UMIN = -10; + double g_x = 1; + double accel_d = 1; + + double vel = 0; + double vel_d = 5; + double distance = 0.1; + + PIDLin pid(FB_LIN_UMAX, FB_LIN_UMIN, params); + + EXPECT_LE(pid.calculateManipulations(vel, vel_d, accel_d), FB_LIN_UMAX); + EXPECT_GE(pid.calculateManipulations(vel, -vel_d, accel_d), FB_LIN_UMIN); + + FB_LIN_UMIN = 0; + pid = PIDLin(FB_LIN_UMAX, FB_LIN_UMIN, params); + + EXPECT_GE(pid.calculateManipulations(vel, -vel_d, accel_d), FB_LIN_UMIN); +} \ No newline at end of file diff --git a/tests/controllers/feedback_linearization/fblin_test.cpp b/tests/controllers/feedback_linearization/fblin_test.cpp new file mode 100644 index 0000000..43a291b --- /dev/null +++ b/tests/controllers/feedback_linearization/fblin_test.cpp @@ -0,0 +1,51 @@ +#include +#include "controllers/feedback_linearization/base/fb_lin_control.hpp" + +TEST(FBLin, SimpleModel){ + double f_x{5}; + double g_x{1}; + + double U_MAX{255}; + double U_MIN{-255}; + double u_aux{10}; + double u_n{2}; + double chiX_dot_d{1}; + + FBLin controller(U_MAX, U_MIN); + controller.f_x_ = f_x; + controller.g_x_ = g_x; + + controller.u_n_ = u_n; + controller.u_aux_ = u_aux; + controller.chiX_dot_d_ = chiX_dot_d; + + controller.updateControlSignal(); + // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux) + double expected_u = -12; + EXPECT_EQ(controller.u_, expected_u); +} + +TEST(FBLin, ClampU){ + double f_x{5}; + double g_x{1}; + + double U_MAX{255}; + double U_MIN{-255}; + double u_aux{300}; + double u_n{2}; + double chiX_dot_d{1}; + + FBLin controller(U_MAX, U_MIN); + controller.f_x_ = f_x; + controller.g_x_ = g_x; + + controller.u_n_ = u_n; + controller.u_aux_ = u_aux; + controller.chiX_dot_d_ = chiX_dot_d; + + controller.updateControlSignal(); + // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux) + + EXPECT_LE(controller.u_, U_MAX); + EXPECT_GE(controller.u_, U_MIN); +} \ No newline at end of file diff --git a/tests/controllers/feedback_linearization/vtec_sdc1_pid_test.cpp b/tests/controllers/feedback_linearization/vtec_sdc1_pid_test.cpp new file mode 100644 index 0000000..c34dee4 --- /dev/null +++ b/tests/controllers/feedback_linearization/vtec_sdc1_pid_test.cpp @@ -0,0 +1,51 @@ +// #include +// #include "controllers/feedback_linearization/base/fb_lin_control.hpp" + +// TEST(FBLin, SimpleModel){ +// double f_x{5}; +// double g_x{1}; + +// double U_MAX{255}; +// double U_MIN{-255}; +// double u_aux{10}; +// double u_n{2}; +// double chiX_dot_d{1}; + +// FBLin controller(U_MAX, U_MIN); +// controller.f_x_ = f_x; +// controller.g_x_ = g_x; + +// controller.u_n_ = u_n; +// controller.u_aux_ = u_aux; +// controller.chiX_dot_d_ = chiX_dot_d; + +// controller.updateControlSignal(); +// // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux) + +// EXPECT_EQ(controller.u_, -12); +// } + +// TEST(FBLin, ClampU){ +// double f_x{5}; +// double g_x{1}; + +// double U_MAX{255}; +// double U_MIN{-255}; +// double u_aux{300}; +// double u_n{2}; +// double chiX_dot_d{1}; + +// FBLin controller(U_MAX, U_MIN); +// controller.f_x_ = f_x; +// controller.g_x_ = g_x; + +// controller.u_n_ = u_n; +// controller.u_aux_ = u_aux; +// controller.chiX_dot_d_ = chiX_dot_d; + +// controller.updateControlSignal(); +// // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux) + +// EXPECT_LE(controller.u_, U_MAX); +// EXPECT_GE(controller.u_, U_MIN); +// } \ No newline at end of file