diff --git a/src/config/ConfigurationName.h b/src/config/ConfigurationName.h index 273398a..17b5f6c 100644 --- a/src/config/ConfigurationName.h +++ b/src/config/ConfigurationName.h @@ -18,6 +18,11 @@ enum class ConfigurationName { kControllerProportionalGain, kControllerIntegrativeGain, kControllerDerivativeGain, + kControllerMassPendulum, + kControllerLengthPendulum, + kControllerLengthRotatingArm, + kControllerMomentInertiaPendulum, + kControllerMomentInertiaRotatingArm }; /** @@ -53,6 +58,16 @@ inline std::string ToString(ConfigurationName config_name) return "kControllerIntegrativeGain"; case ConfigurationName::kControllerDerivativeGain: return "kControllerDerivativeGain"; + case ConfigurationName::kControllerMassPendulum: + return "kControllerMassPendulum"; + case ConfigurationName::kControllerLengthPendulum: + return "kControllerLengthPendulum"; + case ConfigurationName::kControllerLengthRotatingArm: + return "kControllerLengthRotatingArm"; + case ConfigurationName::kControllerMomentInertiaPendulum: + return "kControllerMomentInertiaPendulum"; + case ConfigurationName::kControllerMomentInertiaRotatingArm: + return "kControllerMomentInertiaRotatingArm"; } // Don't add the default case, so that the compiler can warn you. return {}; } @@ -92,6 +107,21 @@ inline ConfigurationName FromString(const std::string& config_name) if (config_name == "kControllerDerivativeGain") { return ConfigurationName::kControllerDerivativeGain; } + if (config_name == "kControllerMassPendulum") { + return ConfigurationName::kControllerMassPendulum; + } + if (config_name == "kControllerLengthPendulum") { + return ConfigurationName::kControllerLengthPendulum; + } + if (config_name == "kControllerLengthRotatingArm") { + return ConfigurationName::kControllerLengthRotatingArm; + } + if (config_name == "kControllerMomentInertiaPendulum") { + return ConfigurationName::kControllerMomentInertiaPendulum; + } + if (config_name == "kControllerMomentInertiaRotatingArm") { + return ConfigurationName::kControllerMomentInertiaRotatingArm; + } spdlog::critical("Trying to parse {} as ConfigurationName", config_name); return {}; } diff --git a/src/controller/CMakeLists.txt b/src/controller/CMakeLists.txt index 34e4ab9..0e62db6 100644 --- a/src/controller/CMakeLists.txt +++ b/src/controller/CMakeLists.txt @@ -3,8 +3,10 @@ add_library(ControllerLib) target_sources(ControllerLib PRIVATE ${CMAKE_CURRENT_LIST_DIR}/Controller.cpp + ${CMAKE_CURRENT_LIST_DIR}/TECSController.cpp PUBLIC ${CMAKE_CURRENT_LIST_DIR}/Controller.h + ${CMAKE_CURRENT_LIST_DIR}/TECSController.h ) target_include_directories(ControllerLib diff --git a/src/controller/TECSController.cpp b/src/controller/TECSController.cpp new file mode 100644 index 0000000..b7f1618 --- /dev/null +++ b/src/controller/TECSController.cpp @@ -0,0 +1,104 @@ +#include "TECSController.h" +#include +namespace controller { + +TECSController::TECSController(config::ConfigurationInterface& config, std::chrono::milliseconds cycle_time) + : kp_(config.GetSetting(config::ConfigurationName::kControllerProportionalGain).ValueByType()) + , ki_(config.GetSetting(config::ConfigurationName::kControllerIntegrativeGain).ValueByType()) + , kd_(config.GetSetting(config::ConfigurationName::kControllerDerivativeGain).ValueByType()) + , mp_(config.GetSetting(config::ConfigurationName::kControllerMassPendulum).ValueByType()) + , lp_(config.GetSetting(config::ConfigurationName::kControllerLengthPendulum).ValueByType()) + , lr_(config.GetSetting(config::ConfigurationName::kControllerLengthRotatingArm).ValueByType()) + , jp_(config.GetSetting(config::ConfigurationName::kControllerMomentInertiaPendulum).ValueByType()) + , jr_(config.GetSetting(config::ConfigurationName::kControllerMomentInertiaRotatingArm).ValueByType()) + , current_state_({ 0.0, 0.0, 0.0, 0.0 }) + , energy_error_(0.0) + , energy_error_integral_(0.0) + , previous_energy_error_(0.0) + , output_(0.0) + , cycle_time_(cycle_time) +{ +} + +void TECSController::Read(PendulumState state) +{ + current_state_ = state; +} + +double TECSController::Write() +{ + Calculate(); + return output_; +} + +double TECSController::CalculateTotalEnergy() const +{ + // Calculate potential energy + // For an inverted pendulum, the minimum potential energy is when the pendulum is hanging down (θ = π) + // and maximum when it's perfectly upright (θ = 0) + double potential_energy = mp_ * g_ * lp_ * (1 + cos(current_state_.theta)); + + // Calculate kinetic energy + // Kinetic energy includes both rotational arm and pendulum contributions + double pendulum_kinetic = 0.5 * jp_ * std::pow(current_state_.theta_dot, 2) + 0.5 * mp_ * (std::pow(lr_ * current_state_.alpha_dot, 2) + std::pow(lp_ * current_state_.theta_dot, 2) + 2 * lr_ * lp_ * current_state_.alpha_dot * current_state_.theta_dot * std::cos(current_state_.theta)); + + double arm_kinetic = 0.5 * jr_ * std::pow(current_state_.alpha_dot, 2); + double kinetic_energy = pendulum_kinetic + arm_kinetic; + + // Total energy is the sum of potential and kinetic + return potential_energy + kinetic_energy; +} + +void TECSController::Calculate() +{ + // Calculate the current total energy of the system + double current_energy = CalculateTotalEnergy(); + + // For the inverted pendulum, the target energy is just the potential energy when upright + double target_energy = mp_ * g_ * lp_ * 2; // Maximum potential energy when θ = 0 + + // Calculate energy error + energy_error_ = target_energy - current_energy; + + // Calculate PID terms based on energy error + double proportional_part = kp_ * energy_error_; + + // Integrate error (with time factor for correct units) + energy_error_integral_ += energy_error_ * static_cast(cycle_time_.count()) / 1000.0; + double integral_part = ki_ * energy_error_integral_; + + // Derivative term uses change in error divided by time + double derivative_part = kd_ * (energy_error_ - previous_energy_error_) / (static_cast(cycle_time_.count()) / 1000.0); + previous_energy_error_ = energy_error_; + + // The control output is the torque to apply to the arm + // We also need to add damping to the arm's angular velocity for stability + double damping_term = -0.1 * current_state_.alpha_dot; + + // Combine all terms + output_ = proportional_part + integral_part + derivative_part + damping_term; + + // Apply swing-up control strategy when pendulum is not near upright + // and balancing control when it's close to upright + if (std::abs(current_state_.theta) > 0.3) { // If more than ~17 degrees from vertical + // Swing-up using energy control + output_ = std::copysign(std::min(std::abs(output_), 1.0), output_); + } + else { + // Near upright position, use LQR-like control directly on angles + double position_term = -3.0 * current_state_.theta - 0.5 * current_state_.theta_dot; + double velocity_term = -0.2 * current_state_.alpha_dot; + output_ = position_term + velocity_term; + } + + // Limit the output to avoid excessive torque + const double max_output = 5.0; + if (output_ > max_output) { + output_ = max_output; + } + if (output_ < -max_output) { + output_ = -max_output; + } +} + +} // namespace controller diff --git a/src/controller/TECSController.h b/src/controller/TECSController.h new file mode 100644 index 0000000..b199713 --- /dev/null +++ b/src/controller/TECSController.h @@ -0,0 +1,60 @@ +#ifndef TECSCONTROLLER_H +#define TECSCONTROLLER_H + +#include "ConfigurationInterface.h" +#include "ControllerInterface.h" +#include +#include + +namespace controller { + +// Specialized interface for Furuta pendulum control +// For TECS, we need to track both angles and their derivatives +struct PendulumState { + double alpha; // Arm angle + double alpha_dot; // Arm angular velocity + double theta; // Pendulum angle + double theta_dot; // Pendulum angular velocity +}; + +class TECSController : public ControllerInterface { +public: + TECSController(config::ConfigurationInterface& config, std::chrono::milliseconds cycle_time); + ~TECSController() override = default; + + // Read current pendulum state + void Read(PendulumState state) override; + + // Write control output (torque applied to the arm) + [[nodiscard]] double Write() override; + +private: + void Calculate(); + [[nodiscard]] double CalculateTotalEnergy() const; + + // Controller parameters + double kp_; // Proportional gain + double ki_; // Integral gain + double kd_; // Derivative gain + + // System parameters + double mp_; // Mass of the pendulum + double lp_; // Length of the pendulum + double lr_; // Length of the rotating arm + double jp_; // Moment of inertia of the pendulum + double jr_; // Moment of inertia of the rotating arm + const double g_ = 9.81; // Gravity acceleration + + // State variables + PendulumState current_state_; + double energy_error_; + double energy_error_integral_; + double previous_energy_error_; + double output_; + + std::chrono::milliseconds cycle_time_; +}; + +} // namespace controller + +#endif diff --git a/test/controller/CMakeLists.txt b/test/controller/CMakeLists.txt index 9ec5a34..24f8f9f 100644 --- a/test/controller/CMakeLists.txt +++ b/test/controller/CMakeLists.txt @@ -1,5 +1,6 @@ set(ControllerTestSources ControllerTest.cpp + TECSControllerTest.cpp ) add_executable(ControllerTest ${ControllerTestSources}) diff --git a/test/controller/TECSControllerTest.cpp b/test/controller/TECSControllerTest.cpp new file mode 100644 index 0000000..fd7ec73 --- /dev/null +++ b/test/controller/TECSControllerTest.cpp @@ -0,0 +1,619 @@ +#include "TECSController.h" +#include "MockConfig.h" +#include "Setting.h" +#include +#include +#include +#include + +using namespace testing; +using namespace controller; +using namespace config; + +class TECSControllerTest : public ::testing::Test { +protected: + void SetUp() override + { + mockConfig_ = std::make_unique(); + + // Set up default expectations for configuration + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerProportionalGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerProportionalGain, + 2.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerIntegrativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerIntegrativeGain, + 0.5, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerDerivativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerDerivativeGain, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerMassPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMassPendulum, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerLengthPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthPendulum, + 0.3, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerLengthRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthRotatingArm, + 0.2, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerMomentInertiaPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaPendulum, + 0.01, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*mockConfig_, GetSetting(ConfigurationName::kControllerMomentInertiaRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaRotatingArm, + 0.02, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + + controller_ = std::make_unique(*mockConfig_, std::chrono::milliseconds(10)); + } + + std::unique_ptr mockConfig_; + std::unique_ptr controller_; +}; + +// Test initialization and energy calculation +TEST_F(TECSControllerTest, InitializationAndEnergyCalculation) +{ + // Initialize with pendulum at rest, hanging down (theta = M_PI) + PendulumState state { 0.0, 0.0, M_PI, 0.0 }; + controller_->Read(state); + + // Write should execute Calculate() and return output + double output = controller_->Write(); + + // For a pendulum hanging down at rest, controller should try to add energy + EXPECT_GT(output, 0.0); +} + +// Test swing-up control when pendulum is far from upright +TEST_F(TECSControllerTest, SwingUpControl) +{ + // Pendulum hanging down (theta = M_PI) with some velocity + PendulumState state { 0.0, 0.0, M_PI, 0.5 }; + controller_->Read(state); + + double output = controller_->Write(); + + // Output should be capped at 1.0 during swing-up + EXPECT_LE(std::abs(output), 1.0); + + // Test with pendulum at different angle but still in swing-up mode + state = { 0.1, 0.1, M_PI - 1.0, 0.2 }; // About 57 degrees from hanging down + controller_->Read(state); + + output = controller_->Write(); + EXPECT_LE(std::abs(output), 1.0); +} + +// Test balancing control when pendulum is near upright +TEST_F(TECSControllerTest, BalancingControl) +{ + // Pendulum nearly upright (theta = 0.1 radians, about 5.7 degrees) + PendulumState state { 0.0, 0.0, 0.1, 0.0 }; + controller_->Read(state); + + double output = controller_->Write(); + + // For small angle from vertical, controller should push in opposite direction of angle + EXPECT_LT(output, 0.0); // Negative output for positive theta + + // Test with small deviation in the other direction + state = { 0.0, 0.0, -0.1, 0.0 }; + controller_->Read(state); + + output = controller_->Write(); + EXPECT_GT(output, 0.0); // Positive output for negative theta +} + +// Test with heavy pendulum +TEST_F(TECSControllerTest, HeavyPendulumTest) +{ + // Create a new test instance with a heavier pendulum + auto heavy_config = std::make_unique(); + + // Set up expectations for heavy pendulum + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerProportionalGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerProportionalGain, + 2.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerIntegrativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerIntegrativeGain, + 0.5, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerDerivativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerDerivativeGain, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerMassPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMassPendulum, + 0.5, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); // 5x heavier + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerLengthPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthPendulum, + 0.3, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerLengthRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthRotatingArm, + 0.2, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerMomentInertiaPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaPendulum, + 0.01, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*heavy_config, GetSetting(ConfigurationName::kControllerMomentInertiaRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaRotatingArm, + 0.02, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + + auto heavy_controller = std::make_unique(*heavy_config, std::chrono::milliseconds(10)); + + // Test with pendulum at 90 degrees + PendulumState state { 0.0, 0.0, M_PI / 2, 0.0 }; + heavy_controller->Read(state); + double heavy_output = heavy_controller->Write(); + + // Compare with standard controller + controller_->Read(state); + double standard_output = controller_->Write(); + + // Heavier pendulum should require more torque (larger absolute output) + EXPECT_GT(std::abs(heavy_output), std::abs(standard_output)); +} + +// Test with longer pendulum +TEST_F(TECSControllerTest, LongPendulumTest) +{ + // Create a new test instance with a longer pendulum + auto long_config = std::make_unique(); + + // Set up expectations for long pendulum + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerProportionalGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerProportionalGain, + 2.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerIntegrativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerIntegrativeGain, + 0.5, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerDerivativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerDerivativeGain, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerMassPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMassPendulum, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerLengthPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthPendulum, + 0.6, // 2x longer + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerLengthRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthRotatingArm, + 0.2, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerMomentInertiaPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaPendulum, + 0.01, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*long_config, GetSetting(ConfigurationName::kControllerMomentInertiaRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaRotatingArm, + 0.02, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + + auto long_controller = std::make_unique(*long_config, std::chrono::milliseconds(10)); + + // Test with pendulum at 90 degrees + PendulumState state { 0.0, 0.0, M_PI / 2, 0.0 }; + long_controller->Read(state); + double long_output = long_controller->Write(); + + // Compare with standard controller + controller_->Read(state); + double standard_output = controller_->Write(); + + // Longer pendulum should require more torque + EXPECT_GT(std::abs(long_output), std::abs(standard_output)); +} + +// Test output limiting +TEST_F(TECSControllerTest, OutputLimitingTest) +{ + // Create a new test instance with high gains + auto high_gain_config = std::make_unique(); + + // Set up expectations with extreme PID values + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerProportionalGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerProportionalGain, + 100.0, // High P gain + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerIntegrativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerIntegrativeGain, + 50.0, // High I gain + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerDerivativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerDerivativeGain, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerMassPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMassPendulum, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerLengthPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthPendulum, + 0.3, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerLengthRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthRotatingArm, + 0.2, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerMomentInertiaPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaPendulum, + 0.01, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*high_gain_config, GetSetting(ConfigurationName::kControllerMomentInertiaRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaRotatingArm, + 0.02, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + + auto high_gain_controller = std::make_unique(*high_gain_config, std::chrono::milliseconds(10)); + + // Large angle that will generate large control output + PendulumState state { 1.0, 2.0, M_PI - 0.1, -3.0 }; + high_gain_controller->Read(state); + + double output = high_gain_controller->Write(); + + // Output should be limited to max value (5.0) + EXPECT_LE(std::abs(output), 5.0); +} + +// Test PID behavior over time +TEST_F(TECSControllerTest, PIDTimeResponseTest) +{ + // Create controllers with different PID configurations + + // P-only controller + auto p_config = std::make_unique(); + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerProportionalGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerProportionalGain, + 1.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerIntegrativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerIntegrativeGain, + 0.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); // No I term + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerDerivativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerDerivativeGain, + 0.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); // No D term + // Set remaining parameters same as default + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerMassPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMassPendulum, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerLengthPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthPendulum, + 0.3, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerLengthRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthRotatingArm, + 0.2, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerMomentInertiaPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaPendulum, + 0.01, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*p_config, GetSetting(ConfigurationName::kControllerMomentInertiaRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaRotatingArm, + 0.02, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + + auto p_controller = std::make_unique(*p_config, std::chrono::milliseconds(10)); + + // PI controller + auto pi_config = std::make_unique(); + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerProportionalGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerProportionalGain, + 1.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerIntegrativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerIntegrativeGain, + 0.5, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); // With I term + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerDerivativeGain)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerDerivativeGain, + 0.0, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); // No D term + // Set remaining parameters same as default + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerMassPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMassPendulum, + 0.1, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerLengthPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthPendulum, + 0.3, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerLengthRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerLengthRotatingArm, + 0.2, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerMomentInertiaPendulum)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaPendulum, + 0.01, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + EXPECT_CALL(*pi_config, GetSetting(ConfigurationName::kControllerMomentInertiaRotatingArm)) + .WillRepeatedly(Return(Setting { + ConfigurationName::kControllerMomentInertiaRotatingArm, + 0.02, + std::nullopt, + std::nullopt, + std::nullopt, + std::nullopt })); + + auto pi_controller = std::make_unique(*pi_config, std::chrono::milliseconds(10)); + + // Start with pendulum at 45 degrees + PendulumState state { 0.0, 0.0, M_PI / 4, 0.0 }; + + // First output with only proportional term + p_controller->Read(state); + double p_output = p_controller->Write(); + + // First output with PI controller + pi_controller->Read(state); + double pi_output1 = pi_controller->Write(); + + // Initially outputs should be similar (mostly P term) + EXPECT_NEAR(p_output, pi_output1, 0.01); + + // Simulate multiple cycles with the same error to accumulate integral term + for (int i = 0; i < 10; i++) { + pi_controller->Read(state); + (void)pi_controller->Write(); + } + + // Final output with accumulated integral term + double pi_output2 = pi_controller->Write(); + + // Integral action should make the control output stronger over time + EXPECT_GT(std::abs(pi_output2), std::abs(pi_output1)); +} + +// Test different pendulum positions and velocities +TEST_F(TECSControllerTest, DifferentPendulumPositionsTest) +{ + // Test case array with different pendulum states and expected output signs + struct TestCase { + PendulumState state; + bool expectPositiveOutput; + }; + + std::vector test_cases = { + // Pendulum hanging down, stationary + { { 0.0, 0.0, M_PI, 0.0 }, true }, // Should try to add energy + + // Pendulum upright, slight angle + { { 0.0, 0.0, 0.1, 0.0 }, false }, // Should push back toward center + { { 0.0, 0.0, -0.1, 0.0 }, true }, // Should push back toward center + + // Pendulum with velocity + { { 0.0, 0.0, M_PI / 2, 1.0 }, true }, // Moving up, should add energy + { { 0.0, 0.0, M_PI / 2, -1.0 }, false }, // Moving down, should try to reverse + + // Pendulum with arm rotation + { { 0.0, 0.5, M_PI / 2, 0.0 }, false }, // Should apply damping to arm + { { 0.0, -0.5, M_PI / 2, 0.0 }, true } // Should apply damping to arm + }; + + for (const auto& tc : test_cases) { + controller_->Read(tc.state); + double output = controller_->Write(); + + if (tc.expectPositiveOutput) { + EXPECT_GT(output, 0.0) << "Failed with state: alpha=" << tc.state.alpha + << ", alpha_dot=" << tc.state.alpha_dot + << ", theta=" << tc.state.theta + << ", theta_dot=" << tc.state.theta_dot; + } + else { + EXPECT_LT(output, 0.0) << "Failed with state: alpha=" << tc.state.alpha + << ", alpha_dot=" << tc.state.alpha_dot + << ", theta=" << tc.state.theta + << ", theta_dot=" << tc.state.theta_dot; + } + } +} \ No newline at end of file