10#include <wpi/math/controller/LinearQuadraticRegulator.hpp>
11#include <wpi/math/estimator/KalmanFilter.hpp>
12#include <wpi/math/system/DCMotor.hpp>
13#include <wpi/math/system/LinearSystem.hpp>
14#include <wpi/math/system/LinearSystemLoop.hpp>
15#include <wpi/math/system/Models.hpp>
16#include <wpi/units/time.hpp>
17#include <wpi/units/voltage.hpp>
38 using Loop1 = wpi::math::LinearSystemLoop<1, 1, 1>;
40 using Loop2 = wpi::math::LinearSystemLoop<2, 1, 1>;
51 wpi::math::LinearSystem<States, 1, 1>
plant;
52 wpi::math::LinearQuadraticRegulator<States, 1>
controller;
53 wpi::math::KalmanFilter<States, 1, 1>
observer;
54 wpi::math::LinearSystemLoop<States, 1, 1>
loop;
67 LoopBundle(
const wpi::math::LinearSystem<States, 1, 1>& system,
const wpi::util::array<double, States>& qElems,
68 const wpi::util::array<double, 1>& rElems,
const wpi::util::array<double, States>& stateStdDevs,
69 const wpi::util::array<double, 1>& measStdDevs, wpi::units::volt_t maxV, wpi::units::second_t dt)
267 std::variant<wpi::math::LinearSystem<1, 1, 1>, wpi::math::LinearSystem<2, 1, 1>>
GetSystem()
275 std::variant<std::unique_ptr<LoopBundle<1>>, std::unique_ptr<LoopBundle<2>>>
GetLoop()
const;
281 static std::vector<double> Checked(
const std::vector<double>& values,
size_t size,
const char* name);
283 std::optional<LQRType> m_type;
284 std::optional<wpi::math::LinearSystem<1, 1, 1>> m_flywheelPlant;
285 std::optional<wpi::math::LinearSystem<2, 1, 1>> m_armElevatorPlant;
287 std::vector<double> m_qElems;
288 std::vector<double> m_rElems;
289 std::vector<double> m_stateStdDevs;
290 std::vector<double> m_measStdDevs;
292 wpi::units::volt_t m_maxVoltage{wpi::units::volt_t{12.0}};
293 wpi::units::second_t m_period{wpi::units::second_t{0.020}};
Definition LQRConfig.hpp:28
LQRConfig & WithStateStdDevs(std::initializer_list< double > stateStdDevs)
LQRConfig & WithElevatorSystem(const wpi::math::DCMotor &motor, double mass, double drumRadius, double gearing)
std::variant< std::unique_ptr< LoopBundle< 1 > >, std::unique_ptr< LoopBundle< 2 > > > GetLoop() const
LQRConfig & WithMaxVoltage(wpi::units::volt_t maxVoltage)
LQRConfig & WithQElems(std::initializer_list< double > qElems)
LQRConfig & WithArmSystem(const wpi::math::DCMotor &motor, double momentOfInertia, double gearing)
wpi::math::LinearSystemLoop< 2, 1, 1 > Loop2
Definition LQRConfig.hpp:40
LQRConfig & WithType(LQRType type)
std::vector< double > GetMeasurementStdDevs() const
wpi::units::second_t GetPeriod() const
LQRType
Definition LQRConfig.hpp:31
@ ELEVATOR
Elevator (2-state position+velocity model).
Definition LQRConfig.hpp:34
@ FLYWHEEL
Flywheel (1-state velocity model).
Definition LQRConfig.hpp:32
@ ARM
Single-jointed arm (2-state position+velocity model).
Definition LQRConfig.hpp:33
LQRConfig & WithRElems(std::initializer_list< double > rElems)
wpi::units::volt_t GetMaxVoltage() const
std::variant< wpi::math::LinearSystem< 1, 1, 1 >, wpi::math::LinearSystem< 2, 1, 1 > > GetSystem() const
LQRConfig & WithStateStdDevs(const std::vector< double > &stateStdDevs)
LQRConfig & WithMeasurementStdDevs(std::initializer_list< double > measStdDevs)
LQRConfig & WithMeasurementStdDevs(const std::vector< double > &measStdDevs)
std::vector< double > GetQElems() const
LQRConfig & WithQElems(const std::vector< double > &qElems)
std::vector< double > GetStateStdDevs() const
LQRConfig & WithPeriod(wpi::units::second_t period)
friend class LQRController
Definition LQRConfig.hpp:278
std::vector< double > GetRElems() const
wpi::math::LinearSystemLoop< 1, 1, 1 > Loop1
Definition LQRConfig.hpp:38
LQRConfig & WithFlywheelSystem(const wpi::math::DCMotor &motor, double momentOfInertia, double gearing)
LQRConfig & WithRElems(const std::vector< double > &rElems)
Definition GearBox.hpp:11
Definition ADRCConfig.hpp:24
LoopBundle(const LoopBundle &)=delete
LoopBundle(LoopBundle &&)=delete
wpi::math::LinearSystemLoop< States, 1, 1 > loop
Definition LQRConfig.hpp:54
wpi::math::KalmanFilter< States, 1, 1 > observer
Definition LQRConfig.hpp:53
LoopBundle(const wpi::math::LinearSystem< States, 1, 1 > &system, const wpi::util::array< double, States > &qElems, const wpi::util::array< double, 1 > &rElems, const wpi::util::array< double, States > &stateStdDevs, const wpi::util::array< double, 1 > &measStdDevs, wpi::units::volt_t maxV, wpi::units::second_t dt)
Definition LQRConfig.hpp:67
LoopBundle & operator=(LoopBundle &&)=delete
LoopBundle & operator=(const LoopBundle &)=delete
wpi::math::LinearQuadraticRegulator< States, 1 > controller
Definition LQRConfig.hpp:52
wpi::math::LinearSystem< States, 1, 1 > plant
Definition LQRConfig.hpp:51