YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
LQRController.hpp
Go to the documentation of this file.
1// Copyright (c) 2026 Yet Another Software Suite
2// SPDX-License-Identifier: LGPL-3.0-or-later
3
4#pragma once
5
6#include <memory>
7#include <mutex>
8#include <optional>
9#include <utility>
10#include <variant>
11#include <wpi/math/controller/LinearQuadraticRegulator.hpp>
12#include <wpi/math/estimator/KalmanFilter.hpp>
13#include <wpi/math/system/LinearSystem.hpp>
14#include <wpi/units/angle.hpp>
15#include <wpi/units/angular_velocity.hpp>
16#include <wpi/units/length.hpp>
17#include <wpi/units/time.hpp>
18#include <wpi/units/velocity.hpp>
19#include <wpi/units/voltage.hpp>
20
21#include "LQRConfig.hpp"
22
23namespace yams::math {
24
37 public:
43 explicit LQRController(LQRConfig config);
44
50 void UpdateConfig(LQRConfig config);
51
60 void Retune(LQRConfig config);
61
68 void Reset(wpi::units::radian_t angle, wpi::units::radians_per_second_t velocity);
69
76 void Reset(wpi::units::meter_t distance, wpi::units::meters_per_second_t velocity);
77
86 wpi::units::volt_t Calculate(wpi::units::radian_t measured, wpi::units::radian_t position,
87 wpi::units::radians_per_second_t velocity);
88
97 wpi::units::volt_t Calculate(wpi::units::meter_t measured, wpi::units::meter_t position,
98 wpi::units::meters_per_second_t velocity);
99
107 wpi::units::volt_t Calculate(wpi::units::radians_per_second_t measured,
108 wpi::units::radians_per_second_t velocity);
109
117 wpi::units::volt_t Calculate(wpi::units::meters_per_second_t measured,
118 wpi::units::meters_per_second_t velocity);
119
126
132 const std::optional<LQRConfig>& GetConfig() const;
133
134 private:
135 // Each loop with the plant, LQR and Kalman filter it points to, so none of its pointers dangle.
136 using FlywheelBundle = LQRConfig::LoopBundle<1>;
137 using ArmElevatorBundle = LQRConfig::LoopBundle<2>;
138
139 static std::unique_ptr<FlywheelBundle> BuildFlywheel(const LQRConfig& cfg);
140 static std::unique_ptr<ArmElevatorBundle> BuildArmElevator(const LQRConfig& cfg);
141
142 std::optional<LQRConfig> m_config;
143 LQRConfig::LQRType m_type;
144 std::variant<std::unique_ptr<FlywheelBundle>, std::unique_ptr<ArmElevatorBundle>> m_bundle;
145 wpi::units::second_t m_period;
146 // Calculate() runs on the closed loop controller's thread while Retune() swaps the loop on the robot's. Held on
147 // the heap so the controller can still be moved.
148 std::unique_ptr<std::mutex> m_mutex{std::make_unique<std::mutex>()};
149};
150
151} // namespace yams::math
Definition LQRConfig.hpp:28
LQRType
Definition LQRConfig.hpp:31
wpi::units::volt_t Calculate(wpi::units::radian_t measured, wpi::units::radian_t position, wpi::units::radians_per_second_t velocity)
void UpdateConfig(LQRConfig config)
LQRConfig::LQRType GetType() const
void Retune(LQRConfig config)
void Reset(wpi::units::meter_t distance, wpi::units::meters_per_second_t velocity)
wpi::units::volt_t Calculate(wpi::units::radians_per_second_t measured, wpi::units::radians_per_second_t velocity)
wpi::units::volt_t Calculate(wpi::units::meters_per_second_t measured, wpi::units::meters_per_second_t velocity)
void Reset(wpi::units::radian_t angle, wpi::units::radians_per_second_t velocity)
wpi::units::volt_t Calculate(wpi::units::meter_t measured, wpi::units::meter_t position, wpi::units::meters_per_second_t velocity)
const std::optional< LQRConfig > & GetConfig() const
LQRController(LQRConfig config)
Definition ADRCConfig.hpp:24
Definition LQRConfig.hpp:50