diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 85bdcf32a2..f985960e03 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -4,6 +4,10 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.system.plant.LinearSystemId; + /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */ @SuppressWarnings("MemberName") public class SimpleMotorFeedforward { @@ -47,6 +51,24 @@ public class SimpleMotorFeedforward { return ks * Math.signum(velocity) + kv * velocity + ka * acceleration; } + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. + * @param dtSeconds Time between velocity setpoints in seconds. + * @return The computed feedforward. + */ + public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { + var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); + var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); + + var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); + var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + + return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index b599b92e1f..da125de18a 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -6,6 +6,9 @@ #include +#include "Eigen/Core" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/system/plant/LinearSystemId.h" #include "units/time.h" #include "units/voltage.h" @@ -52,6 +55,31 @@ class SimpleMotorFeedforward { return kS * wpi::sgn(velocity) + 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 currentVelocity, + units::unit_t nextVelocity, + units::second_t dt) const { + auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); + LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; + + Eigen::Matrix r; + r << currentVelocity.template to(); + + Eigen::Matrix nextR; + nextR << nextVelocity.template to(); + + return kS * wpi::sgn(currentVelocity.template to()) + + units::volt_t{feedforward.Calculate(r, nextR)(0)}; + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java new file mode 100644 index 0000000000..83fced20c6 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import org.junit.jupiter.api.Test; + +class SimpleMotorFeedforwardTest { + @SuppressWarnings("LocalVariableName") + @Test + void testCalculate() { + double Ks = 0.5; + double Kv = 3.0; + double Ka = 0.6; + double dt = 0.02; + + var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka); + var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka); + + var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); + var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka); + + var r = VecBuilder.fill(2.0); + var nextR = VecBuilder.fill(3.0); + + assertEquals(37.524995834325161 + 0.5, simpleMotor.calculate(2.0, 3.0, dt), 0.002); + assertEquals( + plantInversion.calculate(r, nextR).get(0, 0) + Ks, + simpleMotor.calculate(2.0, 3.0, dt), + 0.002); + + // These won't match exactly. It's just an approximation to make sure they're + // in the same ballpark. + assertEquals( + plantInversion.calculate(r, nextR).get(0, 0) + Ks, + simpleMotor.calculate(2.0, 1.0 / dt), + 2.0); + } +} diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp new file mode 100644 index 0000000000..c178ddbb4c --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "Eigen/Core" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/controller/SimpleMotorFeedforward.h" +#include "units/acceleration.h" +#include "units/length.h" +#include "units/time.h" + +namespace frc { + +TEST(SimpleMotorFeedforwardTest, Calculate) { + double Ks = 0.5; + double Kv = 3.0; + double Ka = 0.6; + auto dt = 0.02_s; + + Eigen::Matrix A; + A << -Kv / Ka; + + Eigen::Matrix B; + B << 1.0 / Ka; + + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; + frc::SimpleMotorFeedforward simpleMotor{ + units::volt_t{Ks}, units::volt_t{Kv} / 1_mps, + units::volt_t{Ka} / 1_mps_sq}; + + Eigen::Matrix r; + r << 2; + Eigen::Matrix nextR; + nextR << 3; + + EXPECT_NEAR(37.524995834325161 + Ks, + simpleMotor.Calculate(2_mps, 3_mps, dt).to(), 0.002); + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks, + simpleMotor.Calculate(2_mps, 3_mps, dt).to(), 0.002); + + // These won't match exactly. It's just an approximation to make sure they're + // in the same ballpark. + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks, + simpleMotor.Calculate(2_mps, 1_mps / dt).to(), 2.0); +} + +} // namespace frc