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] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

150
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -4,16 +4,15 @@
#pragma once
#include <gcem.hpp>
#include <wpi/MathExtras.h>
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
#include "units/voltage.h"
#include "wpimath/MathShared.h"
namespace frc {
/**
* A helper class that computes feedforward voltages for a simple
* permanent-magnet DC motor.
@@ -35,22 +34,40 @@ class SimpleMotorFeedforward {
* @param kS The static gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @param dt The period in seconds.
*/
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: kS(kS), kV(kV), kA(kA) {
if (kV.value() < 0) {
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
: kS(kS),
kV([&] {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
return units::unit_t<kv_unit>{0};
} else {
return kV;
}
}()),
kA([&] {
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
return units::unit_t<ka_unit>{0};
} else {
return kA;
}
}()) {
if (dt <= 0_ms) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
kV = units::unit_t<kv_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
}
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
kA = units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
"period must be a positive number, got {}!", dt.value());
m_dt = 20_ms;
wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
} else {
m_dt = dt;
}
}
@@ -60,33 +77,101 @@ class SimpleMotorFeedforward {
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
[[deprecated("Use the current/next velocity overload instead.")]]
constexpr units::volt_t Calculate(
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoint.
* Use this method when the setpoint does not change.
*
* @param setpoint The velocity setpoint, in distance per
* second.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> setpoint) const {
return Calculate(setpoint, setpoint);
}
/**
* 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 {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Vectord<1> r{currentVelocity.value()};
Vectord<1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
if (kA == decltype(kA)(0)) {
// Given the following discrete feedforward model
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
//
// We want the feedforward model when kₐ = 0.
//
// Simplify A.
//
// A = −kᵥ/kₐ
//
// As kₐ approaches zero, A approaches -∞.
//
// A = −∞
//
// Simplify A_d.
//
// A_d = eᴬᵀ
// A_d = std::exp(−∞)
// A_d = 0
//
// Simplify B_d.
//
// B_d = A⁻¹(eᴬᵀ - I)B
// B_d = A⁻¹((0) - I)B
// B_d = A⁻¹(-I)B
// B_d = -A⁻¹B
// B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = kₐ/kᵥ(1/kₐ)
// B_d = 1/kᵥ
//
// Substitute these into the feedforward equation.
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
// uₖ = (1/kᵥ)⁺(rₖ₊₁ − (0) rₖ)
// uₖ = kᵥrₖ₊₁
return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
} else {
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = 1.0 / A * (A_d - 1.0) * B;
return kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *
(nextVelocity.value() - A_d * currentVelocity.value())};
}
}
// Rearranging the main equation from the calculate() method yields the
@@ -168,5 +253,10 @@ class SimpleMotorFeedforward {
/** The acceleration gain. */
const units::unit_t<ka_unit> kA;
private:
/** The period. */
units::second_t m_dt;
};
} // namespace frc