TECS based controller

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 {};
 }

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

src/controller/TECSController.cpp

@@ -0,0 +1,104 @@
+#include "TECSController.h"
+#include <cmath>
+namespace controller {
+
+TECSController::TECSController(config::ConfigurationInterface& config, std::chrono::milliseconds cycle_time)
+    : kp_(config.GetSetting(config::ConfigurationName::kControllerProportionalGain).ValueByType<double>())
+    , ki_(config.GetSetting(config::ConfigurationName::kControllerIntegrativeGain).ValueByType<double>())
+    , kd_(config.GetSetting(config::ConfigurationName::kControllerDerivativeGain).ValueByType<double>())
+    , mp_(config.GetSetting(config::ConfigurationName::kControllerMassPendulum).ValueByType<double>())
+    , lp_(config.GetSetting(config::ConfigurationName::kControllerLengthPendulum).ValueByType<double>())
+    , lr_(config.GetSetting(config::ConfigurationName::kControllerLengthRotatingArm).ValueByType<double>())
+    , jp_(config.GetSetting(config::ConfigurationName::kControllerMomentInertiaPendulum).ValueByType<double>())
+    , jr_(config.GetSetting(config::ConfigurationName::kControllerMomentInertiaRotatingArm).ValueByType<double>())
+    , 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<double>(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<double>(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

src/controller/TECSController.h

@@ -0,0 +1,60 @@
+#ifndef TECSCONTROLLER_H
+#define TECSCONTROLLER_H
+
+#include "ConfigurationInterface.h"
+#include "ControllerInterface.h"
+#include <array>
+#include <chrono>
+
+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<PendulumState, double> {
+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

test/controller/CMakeLists.txt

@@ -1,5 +1,6 @@
 set(ControllerTestSources 
     ControllerTest.cpp
+    TECSControllerTest.cpp
     )
 
 add_executable(ControllerTest ${ControllerTestSources})