YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
ADRCController.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 <optional>
8#include <vector>
9
10#include <wpi/math/linalg/EigenCore.hpp>
11#include <wpi/units/angular_velocity.hpp>
12#include <wpi/units/time.hpp>
13#include <wpi/units/voltage.hpp>
14
17
18namespace yams::math {
19
62 public:
69 explicit ADRCController(std::shared_ptr<ADRCConfig> config);
70
80 void Initialize(double b0, wpi::units::second_t period);
81
91 wpi::units::volt_t Step(double reference, double output, const std::vector<double>& refDerivatives = {});
92
101 wpi::units::volt_t Calculate(double measurement, double setpoint);
102
115 wpi::units::volt_t Calculate(double measurement, double setpoint, wpi::units::volt_t appliedOutput);
116
121 struct Reference {
122 double value{0.0};
123 double rate{0.0};
124
126 static Reference Still(double value) { return Reference{value, 0.0}; }
127 };
128
148 wpi::units::volt_t Calculate(double measurement, Reference now, Reference next, wpi::units::volt_t appliedOutput);
149
157 void Reset(double measurement);
158
167 void Reset(const std::vector<double>& xhatInit, wpi::units::volt_t uInit);
168
170 double GetB0() const;
172 wpi::units::radians_per_second_t GetBandwidth() const;
174 wpi::units::radians_per_second_t GetObserverBandwidth() const;
176 wpi::units::second_t GetSettlingTime() const;
178 double GetBandwidthScale() const;
182 std::vector<double> GetEstimatedStates() const;
189 wpi::units::volt_t GetLastOutput() const;
191 int GetSystemOrder() const { return m_systemOrder; }
193 int GetControllerOrder() const { return m_systemOrder + 1; }
195 const std::shared_ptr<ADRCConfig>& GetConfig() const { return m_config; }
197 bool IsInitialized() const { return m_initialized; }
198
199 private:
200 friend class ADRCConfig;
201
210 static double Bandwidth(int systemOrder, double bandwidthScale, wpi::units::second_t settlingTime);
211
223 static ADRCGains ComputeGains(int systemOrder, double wc, double b0, double dt, double backEmfRate = 0.0);
224
226 void RequireInitialized() const;
227
228 std::shared_ptr<ADRCConfig> m_config;
229 int m_systemOrder;
230 Eigen::VectorXd m_xhat;
231 Eigen::VectorXd m_secondaryXhat;
232 double m_uPrev{0.0};
233 std::vector<double> m_uHistory;
234 std::optional<TrackingDifferentiator> m_td;
235 bool m_initialized{false};
236};
237
238} // namespace yams::math
wpi::units::volt_t Calculate(double measurement, Reference now, Reference next, wpi::units::volt_t appliedOutput)
wpi::units::volt_t Calculate(double measurement, double setpoint)
int GetSystemOrder() const
Definition ADRCController.hpp:191
const TrackingDifferentiator * GetTrackingDifferentiator() const
wpi::units::volt_t Calculate(double measurement, double setpoint, wpi::units::volt_t appliedOutput)
void Reset(double measurement)
wpi::units::second_t GetSettlingTime() const
double GetBandwidthScale() const
std::vector< double > GetEstimatedStates() const
ADRCController(std::shared_ptr< ADRCConfig > config)
wpi::units::volt_t GetLastOutput() const
bool IsInitialized() const
Definition ADRCController.hpp:197
void Initialize(double b0, wpi::units::second_t period)
void Reset(const std::vector< double > &xhatInit, wpi::units::volt_t uInit)
friend class ADRCConfig
Definition ADRCController.hpp:200
wpi::units::radians_per_second_t GetObserverBandwidth() const
double GetEstimatedDisturbance() const
wpi::units::radians_per_second_t GetBandwidth() const
const std::shared_ptr< ADRCConfig > & GetConfig() const
Definition ADRCController.hpp:195
int GetControllerOrder() const
Definition ADRCController.hpp:193
wpi::units::volt_t Step(double reference, double output, const std::vector< double > &refDerivatives={})
Definition TrackingDifferentiator.hpp:51
Definition ADRCConfig.hpp:24
Definition ADRCController.hpp:121
double rate
How fast it is changing, per second.
Definition ADRCController.hpp:123
double value
The setpoint.
Definition ADRCController.hpp:122
static Reference Still(double value)
Definition ADRCController.hpp:126
Definition ADRCConfig.hpp:32