LQRConfig.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <optional>
#include <variant>
#include <vector>
#include <wpi/math/controller/LinearQuadraticRegulator.hpp>
#include <wpi/math/estimator/KalmanFilter.hpp>
#include <wpi/math/system/DCMotor.hpp>
#include <wpi/math/system/LinearSystem.hpp>
#include <wpi/math/system/LinearSystemLoop.hpp>
#include <wpi/math/system/Models.hpp>
#include <wpi/units/time.hpp>
#include <wpi/units/voltage.hpp>
 
namespace yams::math {
 
class LQRConfig {
 public:
  enum class LQRType {
    FLYWHEEL,  
    ARM,       
    ELEVATOR,  
  };
 
  using Loop1 = wpi::math::LinearSystemLoop<1, 1, 1>;
  using Loop2 = wpi::math::LinearSystemLoop<2, 1, 1>;
 
  LQRConfig& WithType(LQRType type);
 
  LQRConfig& WithFlywheelSystem(const wpi::math::DCMotor& motor, double momentOfInertia,
                                double gearing);
 
  LQRConfig& WithArmSystem(const wpi::math::DCMotor& motor, double momentOfInertia, double gearing);
 
  LQRConfig& WithElevatorSystem(const wpi::math::DCMotor& motor, double mass, double drumRadius,
                                double gearing);
 
  LQRConfig& WithQElems(std::initializer_list<double> qElems);
 
  LQRConfig& WithRElems(std::initializer_list<double> rElems);
 
  LQRConfig& WithStateStdDevs(std::initializer_list<double> stateStdDevs);
 
  LQRConfig& WithMeasurementStdDevs(std::initializer_list<double> measStdDevs);
 
  LQRConfig& WithMaxVoltage(wpi::units::volt_t maxVoltage);
 
  LQRConfig& WithPeriod(wpi::units::second_t period);
 
  LQRType GetType() const;
 
  wpi::units::second_t GetPeriod() const;
 
  wpi::units::volt_t GetMaxVoltage() const;
 
  std::variant<wpi::math::LinearSystem<1, 1, 1>, wpi::math::LinearSystem<2, 1, 1>> GetSystem()
      const;
 
  std::variant<Loop1, Loop2> GetLoop() const;
 
 private:
  friend class LQRController;
 
  std::optional<LQRType> m_type;
  std::optional<wpi::math::LinearSystem<1, 1, 1>> m_flywheelPlant;
  std::optional<wpi::math::LinearSystem<2, 1, 1>> m_armElevatorPlant;
 
  std::vector<double> m_qElems;
  std::vector<double> m_rElems;
  std::vector<double> m_stateStdDevs;
  std::vector<double> m_measStdDevs;
 
  wpi::units::volt_t m_maxVoltage{wpi::units::volt_t{12.0}};
  wpi::units::second_t m_period{wpi::units::second_t{0.020}};
};
 
}  // namespace yams::math