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

[wpimath] Fix feedforward returning NaN when kᵥ = 0 (#7790)

Browse Source
This commit is contained in:
Tyler Veness
2025-02-25 19:07:51 -08:00
committed by GitHub
parent 75321f1d84
commit 822457d45b
5 changed files with 40 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -202,13 +202,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
*/
public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (ka == 0.0) {
if (ka < 1e-9) {
return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
} else {
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
return kg
+ ks * Math.signum(currentVelocity)
+ 1.0 / B_d * (nextVelocity - A_d * currentVelocity);

4
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -186,13 +186,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
*/
public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (ka == 0.0) {
if (ka < 1e-9) {
return ks * Math.signum(nextVelocity) + kv * nextVelocity;
} else {
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
}
}

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

@@ -133,13 +133,13 @@ class ElevatorFeedforward {
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (kA == decltype(kA)(0)) {
if (kA < decltype(kA)(1e-9)) {
return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
} else {
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;
double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
return kG + kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *

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

@@ -109,13 +109,13 @@ class SimpleMotorFeedforward {
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (kA == decltype(kA)(0)) {
if (kA < decltype(kA)(1e-9)) {
return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
} else {
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;
double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
return kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *