Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fully discretized ElevatorFF and ArmFF (#7024)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-10-11 01:10:45 -04:00
committed by GitHub
parent 5d9a553104
commit 4adfa8bf64
31 changed files with 1004 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -37,11 +37,16 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per radian.
* @param kA The acceleration gain, in volt seconds² per radian.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: kS(kS), kG(kG), kV(kV), kA(kA) {
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
@@ -54,45 +59,84 @@ class WPILIB_DLLEXPORT ArmFeedforward {
this->kA = units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
}
if (dt <= 0_ms) {
wpi::math::MathSharedStore::ReportError(
"period must be a positive number, got {}!", dt.value());
this->m_dt = 20_ms;
wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
}
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param angle The angle setpoint, in radians. This angle should be
* measured from the horizontal (i.e. if the provided
* angle is 0, the arm should be parallel to the floor).
* If your encoder does not follow this convention, an
* offset should be added.
* @param velocity The velocity setpoint, in radians per second.
* @param acceleration The acceleration setpoint, in radians per second².
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward, in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Angle> angle,
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
kV * velocity + kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param currentAngle The current angle in radians. This angle should be
* measured from the horizontal (i.e. if the provided angle is 0, the arm
* should be parallel to the floor). If your encoder does not follow this
* convention, an offset should be added.
* @param currentVelocity The current velocity setpoint in radians per second.
* @param nextVelocity The next velocity setpoint in radians per second.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const;
/**
* Calculates the feedforward from the gains and setpoint assuming discrete
* control. Use this method when the velocity does not change.
*
* @param currentAngle The current angle. This angle should be measured from
* the horizontal (i.e. if the provided angle is 0, the arm should be parallel
* to the floor). If your encoder does not follow this convention, an offset
* should be added.
* @param currentVelocity The current velocity.
* @return The computed feedforward in volts.
*/
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity) const;
/**
* Calculates the feedforward from the gains and setpoints assuming discrete
* control.
*
* @param currentAngle The current angle. This angle should be measured from
* the horizontal (i.e. if the provided angle is 0, the arm should be parallel
* to the floor). If your encoder does not follow this convention, an offset
* should be added.
* @param currentVelocity The current velocity.
* @param nextVelocity The next velocity.
* @return The computed feedforward in volts.
*/
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const;
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
@@ -232,6 +276,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
/// The acceleration gain, in V/(rad/s²).
units::unit_t<ka_unit> kA;
/** The period. */
units::second_t m_dt;
};
} // namespace frc