YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
ADRCConfig.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 <string>
10#include <vector>
11
12#include <wpi/math/linalg/EigenCore.hpp>
13#include <wpi/math/system/DCMotor.hpp>
14#include <wpi/units/time.hpp>
15#include <wpi/units/voltage.hpp>
16#include <wpi/util/Alert.hpp>
17
19
22} // namespace yams::motorcontrollers
23
24namespace yams::math {
25
26class ADRCController;
27
32struct ADRCGains {
33 Eigen::VectorXd ld;
34 Eigen::MatrixXd ad;
35 Eigen::VectorXd bd;
36 Eigen::VectorXd k;
37 double b0{1.0};
39 double backEmfRate{0.0};
40};
41
83 public:
89 enum class SystemOrder {
90 FIRST = 1,
92 SECOND = 2,
93 THIRD = 3,
94 FOURTH = 4,
102 POSITION = 100,
103 ARM = 100,
104 PIVOT = 100,
110 ELEVATOR = 101,
111 };
112
119 static constexpr int GetOrder(SystemOrder order) {
120 return IsChosenWhenBuilt(order) ? 0 : static_cast<int>(order);
121 }
122
129 static constexpr bool IsChosenWhenBuilt(SystemOrder order) {
130 return order == SystemOrder::POSITION || order == SystemOrder::ELEVATOR;
131 }
132
139 static constexpr bool RequiresLinearClosedLoop(SystemOrder order) { return order == SystemOrder::ELEVATOR; }
140
142 static constexpr double kSpeedPerVoltTimeConstantPeriods = 2.0;
143
145 static constexpr wpi::units::second_t kDefaultSettlingTime{0.2};
147 static constexpr double kDefaultBandwidthScale{1.0};
149 static constexpr wpi::units::second_t kDefaultInputDelay{0.0};
150
159 ADRCConfig(const ADRCConfig& other);
168
184 ADRCConfig& WithSettlingTime(wpi::units::second_t settlingTime);
192 ADRCConfig& WithBandwidthScale(double bandwidthScale);
200 ADRCConfig& WithB0(double b0);
208 ADRCConfig& WithInputDelay(wpi::units::second_t inputDelay);
216 ADRCConfig& WithCascaded(bool cascaded);
235 ADRCConfig& WithBackEmfModel(bool backEmfModel);
252
254 std::optional<SystemOrder> GetSystemOrder() const;
256 std::optional<wpi::units::second_t> GetSettlingTime() const;
258 std::optional<double> GetBandwidthScale() const;
260 std::optional<double> GetB0() const;
262 std::optional<wpi::units::second_t> GetInputDelay() const;
264 bool IsCascaded() const;
266 bool IsBackEmfModeled() const;
271 std::optional<wpi::units::second_t> GetModeledBackEmfTimeConstant() const;
273 std::optional<TrackingDifferentiator::Method> GetTdMethod() const;
275 std::vector<double> GetTdParams() const;
276
288 double ResolveB0(const wpi::math::DCMotor& motor, const motorcontrollers::SmartMotorControllerConfig& config) const;
289
304 static ADRCController BuildController(const std::shared_ptr<ADRCConfig>& self, const wpi::math::DCMotor& motor,
306 wpi::units::second_t period);
307
314 void ReportRejected(std::optional<std::string> reason);
315
316 private:
317 friend class ADRCController;
318
320 struct StepValues {
321 std::shared_ptr<const ADRCGains> gains;
322 double period;
323 bool cascaded;
324 };
325
330 struct Plant {
331 int order;
332 bool speedPerVolt;
333 double backEmfTimeConstant;
334 };
336 Plant ChoosePlant(const wpi::math::DCMotor& motor, const motorcontrollers::SmartMotorControllerConfig& config,
337 SystemOrder order, const std::string& owner) const;
339 int PlantOrder() const;
341 int PlantOrderLocked() const;
343 void SetBuiltValues(double b0, wpi::units::second_t period);
345 void RebuildGains();
347 StepValues GetStepValues() const;
349 std::shared_ptr<const ADRCGains> GetGains() const;
351 std::string AlertId(const std::string& alert) const;
352
353 mutable std::mutex m_mutex;
354 std::string m_owner{"ADRC"};
355 std::optional<SystemOrder> m_systemOrder;
356 std::optional<wpi::units::second_t> m_settlingTime;
357 std::optional<double> m_bandwidthScale;
358 std::optional<double> m_b0;
359 std::optional<wpi::units::second_t> m_inputDelay;
360 bool m_cascaded{false};
361 bool m_backEmfModel{true};
362 std::optional<TrackingDifferentiator::Method> m_tdMethod;
363 std::vector<double> m_tdParams;
364
365 std::optional<wpi::units::second_t> m_builtPeriod;
366 std::optional<double> m_builtB0;
367 int m_builtOrder{0};
369 std::optional<double> m_builtBackEmfTimeConstant;
370 std::shared_ptr<const ADRCGains> m_gains;
371
372 std::unique_ptr<wpi::util::Alert> m_rejectedAlert;
373 int m_instance;
374};
375
376} // namespace yams::math
std::optional< TrackingDifferentiator::Method > GetTdMethod() const
std::optional< double > GetBandwidthScale() const
static constexpr double kSpeedPerVoltTimeConstantPeriods
Definition ADRCConfig.hpp:142
ADRCConfig & WithoutTrackingDifferentiator()
ADRCConfig & WithB0(double b0)
static constexpr bool IsChosenWhenBuilt(SystemOrder order)
Definition ADRCConfig.hpp:129
static constexpr int GetOrder(SystemOrder order)
Definition ADRCConfig.hpp:119
std::optional< wpi::units::second_t > GetSettlingTime() const
double ResolveB0(const wpi::math::DCMotor &motor, const motorcontrollers::SmartMotorControllerConfig &config) const
std::optional< wpi::units::second_t > GetInputDelay() const
static constexpr bool RequiresLinearClosedLoop(SystemOrder order)
Definition ADRCConfig.hpp:139
ADRCConfig & WithSystemOrder(SystemOrder order)
ADRCConfig & WithTrackingDifferentiator(TrackingDifferentiator::Method method, std::vector< double > params={})
static constexpr double kDefaultBandwidthScale
Definition ADRCConfig.hpp:147
ADRCConfig(const ADRCConfig &other)
ADRCConfig & WithBackEmfModel(bool backEmfModel)
static constexpr wpi::units::second_t kDefaultInputDelay
Definition ADRCConfig.hpp:149
ADRCConfig & operator=(const ADRCConfig &other)
std::vector< double > GetTdParams() const
SystemOrder
Definition ADRCConfig.hpp:89
@ ELEVATOR
Definition ADRCConfig.hpp:110
@ SECOND
Second order: a position loop whose inertia sets how fast it moves. POSITION chooses for you.
Definition ADRCConfig.hpp:92
@ FIRST
First order: the loop commands and measures velocity, such as a flywheel.
Definition ADRCConfig.hpp:90
@ FLYWHEEL
A flywheel's velocity loop; the same as FIRST.
Definition ADRCConfig.hpp:91
@ ARM
An arm's position loop; the same as POSITION.
Definition ADRCConfig.hpp:103
@ PIVOT
A pivot's or turret's position loop; the same as POSITION.
Definition ADRCConfig.hpp:104
@ POSITION
Definition ADRCConfig.hpp:102
@ FOURTH
Fourth order: three integrations between the voltage and the measurement. Rare.
Definition ADRCConfig.hpp:94
@ THIRD
Third order: the measurement sits two integrations behind the voltage. Rare.
Definition ADRCConfig.hpp:93
ADRCConfig & WithSettlingTime(wpi::units::second_t settlingTime)
void ReportRejected(std::optional< std::string > reason)
ADRCConfig & WithCascaded(bool cascaded)
static constexpr wpi::units::second_t kDefaultSettlingTime
Definition ADRCConfig.hpp:145
std::optional< SystemOrder > GetSystemOrder() const
ADRCConfig & WithBandwidthScale(double bandwidthScale)
friend class ADRCController
Definition ADRCConfig.hpp:317
bool IsCascaded() const
bool IsBackEmfModeled() const
std::optional< double > GetB0() const
ADRCConfig & WithInputDelay(wpi::units::second_t inputDelay)
std::optional< wpi::units::second_t > GetModeledBackEmfTimeConstant() const
static ADRCController BuildController(const std::shared_ptr< ADRCConfig > &self, const wpi::math::DCMotor &motor, const motorcontrollers::SmartMotorControllerConfig &config, wpi::units::second_t period)
Definition ADRCController.hpp:61
Method
Definition TrackingDifferentiator.hpp:54
Definition SmartMotorControllerConfig.hpp:66
Definition ADRCConfig.hpp:24
Definition ADRCConfig.hpp:20
Definition ADRCConfig.hpp:32
Eigen::MatrixXd ad
Discrete ESO state matrix.
Definition ADRCConfig.hpp:34
Eigen::VectorXd ld
ESO observer gains.
Definition ADRCConfig.hpp:33
Eigen::VectorXd bd
Discrete ESO input matrix.
Definition ADRCConfig.hpp:35
double backEmfRate
Definition ADRCConfig.hpp:39
Eigen::VectorXd k
Controller gains.
Definition ADRCConfig.hpp:36
double b0
Input gain the matrices were worked out with, which the control law divides by.
Definition ADRCConfig.hpp:37