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

[wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
narmstro2020
2023-10-09 11:16:45 -04:00
committed by GitHub
parent a789632052
commit faa1e665ba
4 changed files with 122 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -6,6 +6,9 @@
#include <wpi/MathExtras.h>
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/length.h"
#include "units/time.h"
#include "units/voltage.h"
@@ -54,6 +57,50 @@ class ElevatorFeedforward {
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param currentVelocity The current velocity setpoint, in distance per
* second.
* @param nextVelocity The next velocity setpoint, in distance per second.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward, in volts.
*/
units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
// Discretize the affine model.
//
// dx/dt = Ax + Bu + c
// dx/dt = Ax + B(u + B⁺c)
// xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
// xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
// xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
//
// Solve for uₖ.
//
// B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
//
// For an elevator with the model
// dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
// A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
// assuming sgn(x) is a constant for the duration of the step.
//
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Vectord<1> r{currentVelocity.value()};
Vectord<1> nextR{nextVelocity.value()};
return kG + kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below: