Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilibc] Add LinearSystemLoop C++ ctor to match Java (#2755)

Browse Source 
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
This commit is contained in:
Matt
2020-10-02 17:37:26 -07:00
committed by GitHub
parent 330b90e046
commit 21d949daa8
7 changed files with 74 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -37,20 +37,68 @@ class LinearSystemLoop {
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
* call reset with the initial system state before enabling the loop.
* call reset with the initial system state before enabling the loop. This
* constructor assumes that the input(s) to this system are voltage.
*
* @param plant State-space plant.
* @param controller State-space controller.
* @param feedforward Plant inversion feedforward.
* @param observer State-space observer.
* @param maxVoltageVolts The maximum voltage that can be applied. Assumes
* that the inputs are voltages.
* @param maxVoltage The maximum voltage that can be applied. Commonly 12.
* @param dt The nominal timestep.
*/
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
units::volt_t maxVoltage)
units::volt_t maxVoltage, units::second_t dt)
: LinearSystemLoop(
plant, controller, observer,
[=](Eigen::Matrix<double, Inputs, 1> u) {
return frc::NormalizeInputVector<Inputs>(
u, maxVoltage.template to<double>());
},
dt) {}
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
* call reset with the initial system state before enabling the loop. This
* constructor assumes that the input(s) to this system are voltage.
*
* @param plant State-space plant.
* @param controller State-space controller.
* @param observer State-space observer.
* @param clampFunction The function used to clamp the input vector.
* @param dt The nominal timestep.
*/
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Matrix<double, Inputs, 1>(
const Eigen::Matrix<double, Inputs, 1>&)>
clampFunction,
units::second_t dt)
: LinearSystemLoop(
plant, controller,
LinearPlantInversionFeedforward<States, Inputs>{plant, dt},
observer, clampFunction) {}
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
* call reset with the initial system state before enabling the loop.
*
* @param plant State-space plant.
* @param controller State-space controller.
* @param feedforward Plant inversion feedforward.
* @param observer State-space observer.
* @param maxVoltage The maximum voltage that can be applied. Assumes
* that the inputs are voltages.
*/
LinearSystemLoop(
LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(plant, controller, feedforward, observer,
[=](Eigen::Matrix<double, Inputs, 1> u) {
return frc::NormalizeInputVector<Inputs>(
@@ -61,18 +109,20 @@ class LinearSystemLoop {
* Constructs a state-space loop with the given plant, controller, and
* observer.
*
* @param plant State-space plant.
* @param controller State-space controller.
* @param feedforward Plant-inversion feedforward.
* @param observer State-space observer.
* @param plant State-space plant.
* @param controller State-space controller.
* @param feedforward Plant-inversion feedforward.
* @param observer State-space observer.
* @param clampFunction The function used to clamp the input vector.
*/
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Matrix<double, Inputs, 1>(
const Eigen::Matrix<double, Inputs, 1>&)>
clampFunction)
LinearSystemLoop(
LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Matrix<double, Inputs, 1>(
const Eigen::Matrix<double, Inputs, 1>&)>
clampFunction)
: m_plant(plant),
m_controller(controller),
m_feedforward(feedforward),
@@ -82,11 +132,6 @@ class LinearSystemLoop {
Reset(m_nextR);
}
virtual ~LinearSystemLoop() = default;
LinearSystemLoop(LinearSystemLoop&&) = default;
LinearSystemLoop& operator=(LinearSystemLoop&&) = default;
/**
* Returns the observer's state estimate x-hat.
*/
@@ -240,7 +285,7 @@ class LinearSystemLoop {
protected:
LinearSystem<States, Inputs, Outputs>& m_plant;
LinearQuadraticRegulator<States, Inputs>& m_controller;
LinearPlantInversionFeedforward<States, Inputs>& m_feedforward;
LinearPlantInversionFeedforward<States, Inputs> m_feedforward;
KalmanFilter<States, Inputs, Outputs>& m_observer;
/**