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 56fbea9c8b0..0aa130931c8 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,14 +4,12 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.system.plant.LinearSystemId; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.units.Measure; import edu.wpi.first.units.Unit; import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */ public class SimpleMotorFeedforward { @@ -24,14 +22,8 @@ public class SimpleMotorFeedforward { /** The acceleration gain. */ public final double ka; - /** The feedforward. */ - private final LinearPlantInversionFeedforward feedforward; - - /** The current reference. */ - private final Matrix r; - - /** The next reference. */ - private Matrix nextR; + /** The period. */ + private double m_dt; /** * Creates a new SimpleMotorFeedforward with the specified gains and period. Units of the gain @@ -40,12 +32,12 @@ public class SimpleMotorFeedforward { * @param ks The static gain. * @param kv The velocity gain. * @param ka The acceleration gain. - * @param periodSeconds The period in seconds. + * @param dtSeconds The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. * @throws IllegalArgumentException for period ≤ zero. */ - public SimpleMotorFeedforward(double ks, double kv, double ka, double periodSeconds) { + public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) { this.ks = ks; this.kv = kv; this.ka = ka; @@ -55,21 +47,11 @@ public SimpleMotorFeedforward(double ks, double kv, double ka, double periodSeco if (ka < 0.0) { throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } - if (periodSeconds <= 0.0) { + if (dtSeconds <= 0.0) { throw new IllegalArgumentException( - "period must be a positive number, got " + periodSeconds + "!"); - } - if (ka != 0.0) { - var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); - this.feedforward = new LinearPlantInversionFeedforward<>(plant, periodSeconds); - - this.r = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0); - this.nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0); - } else { - this.feedforward = null; - this.r = null; - this.nextR = null; + "period must be a positive number, got " + dtSeconds + "!"); } + m_dt = dtSeconds; } /** @@ -104,6 +86,7 @@ public SimpleMotorFeedforward(double ks, double kv) { * @param velocity The velocity setpoint. * @param acceleration The acceleration setpoint. * @return The computed feedforward. + * @deprecated Use the current/next velocity overload instead. */ @SuppressWarnings("removal") @Deprecated(forRemoval = true, since = "2025") @@ -117,6 +100,7 @@ public double calculate(double velocity, double acceleration) { * * @param velocity The velocity setpoint. * @return The computed feedforward. + * @deprecated Use the current/next velocity overload instead. */ @SuppressWarnings("removal") @Deprecated(forRemoval = true, since = "2025") @@ -132,50 +116,69 @@ public double calculate(double velocity) { * @param nextVelocity The next velocity setpoint. * @return The computed feedforward. */ - public > double calculate( + public > Measure calculate( Measure> currentVelocity, Measure> nextVelocity) { if (ka == 0.0) { - return ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude(); - // 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 = 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ₖ₊₁ + // 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 = 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 Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude()); } else { - r.set(0, 0, currentVelocity.magnitude()); - nextR.set(0, 0, nextVelocity.magnitude()); - - return ks * Math.signum(currentVelocity.magnitude()) - + feedforward.calculate(r, nextR).get(0, 0); + // 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 / 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; + return Volts.of( + ks * Math.signum(currentVelocity.magnitude()) + + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude())); } } diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index b9fa5c90d36..1ec91eba0c6 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -4,16 +4,15 @@ #pragma once +#include #include -#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. @@ -40,8 +39,37 @@ class SimpleMotorFeedforward { SimpleMotorFeedforward( units::volt_t kS, units::unit_t kV, units::unit_t kA = units::unit_t(0), - units::second_t periodSeconds = units::second_t(0.020)); - + 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{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{0}; + } else { + return kA; + } + }()) { + if (dt <= 0_ms) { + wpi::math::MathSharedStore::ReportError( + "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; + } + } /** * Calculates the feedforward from the gains and setpoints. @@ -49,7 +77,9 @@ 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. */ + [[deprecated("Use the current/next velocity overload instead.")]] constexpr units::volt_t Calculate(units::unit_t velocity, units::unit_t acceleration = units::unit_t(0)) const { @@ -64,15 +94,72 @@ class SimpleMotorFeedforward { * @param nextVelocity The next velocity setpoint, in distance per second. * @return The computed feedforward, in volts. */ - units::volt_t Calculate(units::unit_t currentVelocity, - units::unit_t nextVelocity) const { - if (kA.value() == 0.0) { + constexpr units::volt_t Calculate( + units::unit_t currentVelocity, + units::unit_t 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 { - return kS * wpi::sgn(currentVelocity.value()) + - units::volt_t{feedforward.Calculate({currentVelocity.value()}, {nextVelocity.value()})(0)}; + // 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 @@ -155,12 +242,9 @@ class SimpleMotorFeedforward { /** The acceleration gain. */ const units::unit_t kA; - /** The plant. */ - const frc::LinearSystem<1, 1, 1> plant; - - /** The feedforward. */ - const frc::LinearPlantInversionFeedforward<1, 1> feedforward; - - + private: + /** The period. */ + units::second_t m_dt; }; + } // namespace frc 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 index 79dc478a2c9..9d501edf92a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -26,7 +26,7 @@ void testCalculate() { var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka); var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); - var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka); + var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka, dt); var r = VecBuilder.fill(2.0); var nextR = VecBuilder.fill(3.0); @@ -34,17 +34,19 @@ void testCalculate() { var nextVelocity = MutableMeasure.ofBaseUnits(3.0, RadiansPerSecond); assertEquals( - 37.52499583432516 + 0.5, simpleMotor.calculate(currentVelocity, nextVelocity), 0.002); + 37.52499583432516 + 0.5, + simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + 0.002); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity), + simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), 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(currentVelocity, nextVelocity), + simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), 2.0); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 14cdf294714..c1422ada8ea 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -23,8 +23,10 @@ class DifferentialDriveVoltageConstraintTest { @Test void testDifferentialDriveVoltageConstraint() { + var dt = 0.02; + // Pick an unreasonably large kA to ensure the constraint has to do some work - var feedforward = new SimpleMotorFeedforward(1, 1, 3); + var feedforward = new SimpleMotorFeedforward(1, 1, 3, dt); var kinematics = new DifferentialDriveKinematics(0.5); double maxVoltage = 10; var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage); @@ -34,63 +36,40 @@ void testDifferentialDriveVoltageConstraint() { var duration = trajectory.getTotalTimeSeconds(); var t = 0.0; - var dt = 0.02; while (t < duration) { - var point = trajectory.sample(t); - var chassisSpeeds = + var currentPoint = trajectory.sample(t); + var nextPoint = trajectory.sample(t + dt); + t += dt; + + var currentChassisSpeeds = new ChassisSpeeds( - point.velocityMetersPerSecond, + currentPoint.velocityMetersPerSecond, 0, - point.velocityMetersPerSecond * point.curvatureRadPerMeter); + currentPoint.velocityMetersPerSecond * currentPoint.curvatureRadPerMeter); + var currentWheelSpeeds = kinematics.toWheelSpeeds(currentChassisSpeeds); - var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + var nextChassisSpeeds = + new ChassisSpeeds( + nextPoint.velocityMetersPerSecond, + 0, + nextPoint.velocityMetersPerSecond * nextPoint.curvatureRadPerMeter); + var nextWheelSpeeds = kinematics.toWheelSpeeds(nextChassisSpeeds); - t += dt; + var leftVoltage = + feedforward.calculate( + MutableMeasure.ofBaseUnits(currentWheelSpeeds.leftMetersPerSecond, MetersPerSecond), + MutableMeasure.ofBaseUnits(nextWheelSpeeds.leftMetersPerSecond, MetersPerSecond)); + var rightVoltage = + feedforward.calculate( + MutableMeasure.ofBaseUnits(currentWheelSpeeds.leftMetersPerSecond, MetersPerSecond), + MutableMeasure.ofBaseUnits(nextWheelSpeeds.leftMetersPerSecond, MetersPerSecond)); - // Not really a strictly-correct test as we're using the chassis accel instead of the - // wheel accel, but much easier than doing it "properly" and a reasonable check anyway assertAll( - () -> - assertTrue( - feedforward.calculate( - MutableMeasure.ofBaseUnits( - wheelSpeeds.leftMetersPerSecond, MetersPerSecond), - MutableMeasure.ofBaseUnits( - wheelSpeeds.leftMetersPerSecond - + dt * point.accelerationMetersPerSecondSq, - MetersPerSecond)) - <= maxVoltage + 0.05), - () -> - assertTrue( - feedforward.calculate( - MutableMeasure.ofBaseUnits( - wheelSpeeds.leftMetersPerSecond, MetersPerSecond), - MutableMeasure.ofBaseUnits( - wheelSpeeds.leftMetersPerSecond - + dt * point.accelerationMetersPerSecondSq, - MetersPerSecond)) - >= -maxVoltage - 0.05), - () -> - assertTrue( - feedforward.calculate( - MutableMeasure.ofBaseUnits( - wheelSpeeds.rightMetersPerSecond, MetersPerSecond), - MutableMeasure.ofBaseUnits( - wheelSpeeds.rightMetersPerSecond - + dt * point.accelerationMetersPerSecondSq, - MetersPerSecond)) - <= maxVoltage + 0.05), - () -> - assertTrue( - feedforward.calculate( - MutableMeasure.ofBaseUnits( - wheelSpeeds.rightMetersPerSecond, MetersPerSecond), - MutableMeasure.ofBaseUnits( - wheelSpeeds.rightMetersPerSecond - + dt * point.accelerationMetersPerSecondSq, - MetersPerSecond)) - >= -maxVoltage - 0.05)); + () -> assertTrue(leftVoltage.magnitude() <= maxVoltage + 0.05), + () -> assertTrue(leftVoltage.magnitude() >= -maxVoltage - 0.05), + () -> assertTrue(rightVoltage.magnitude() <= maxVoltage + 0.05), + () -> assertTrue(rightVoltage.magnitude() >= -maxVoltage - 0.05)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java index 2c228954dc3..6a987d47445 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -49,7 +49,7 @@ private static ExponentialProfile.State checkDynamics( var nextVelocity = MutableMeasure.ofBaseUnits(next.velocity, RadiansPerSecond); var signal = feedforward.calculate(currentVelocity, nextVelocity); - assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9); + assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9); return next; } diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp index 78780112271..35d35b8f365 100644 --- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp @@ -12,35 +12,34 @@ #include "units/acceleration.h" #include "units/length.h" #include "units/time.h" +#include "units/velocity.h" namespace frc { TEST(SimpleMotorFeedforwardTest, Calculate) { - double Ks = 0.5; - double Kv = 3.0; - double Ka = 0.6; - auto dt = 0.02_s; + constexpr auto Ks = 0.5_V; + constexpr auto Kv = 3_V / 1_mps; + constexpr auto Ka = 0.6_V / 1_mps_sq; + constexpr units::second_t dt = 20_ms; - Matrixd<1, 1> A{-Kv / Ka}; - Matrixd<1, 1> B{1.0 / Ka}; + constexpr Matrixd<1, 1> A{{-Kv.value() / Ka.value()}}; + constexpr Matrixd<1, 1> B{{1.0 / Ka.value()}}; 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}; + frc::SimpleMotorFeedforward simpleMotor{Ks, Kv, Ka}; - Vectord<1> r{2.0}; - Vectord<1> nextR{3.0}; + constexpr Vectord<1> r{{2.0}}; + constexpr Vectord<1> nextR{{3.0}}; - EXPECT_NEAR(37.524995834325161 + Ks, - simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002); - EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks, - simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002); + EXPECT_NEAR((37.524995834325161_V + Ks).value(), + simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002); + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(), + simpleMotor.Calculate(2_mps, 3_mps).value(), 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).value(), 2.0); + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(), + simpleMotor.Calculate(2_mps, 3_mps).value(), 2.0); } } // namespace frc diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp index 5282638ad02..65104b0f3e9 100644 --- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp @@ -21,9 +21,11 @@ using namespace frc; TEST(DifferentialDriveVoltageConstraintTest, Constraint) { + constexpr units::second_t dt = 20_ms; + // Pick an unreasonably large kA to ensure the constraint has to do some work SimpleMotorFeedforward feedforward{1_V, 1_V / 1_mps, - 3_V / 1_mps_sq}; + 3_V / 1_mps_sq, dt}; const DifferentialDriveKinematics kinematics{0.5_m}; const auto maxVoltage = 10_V; @@ -34,35 +36,38 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) { auto trajectory = TestTrajectory::GetTrajectory(config); units::second_t time = 0_s; - units::second_t dt = 20_ms; units::second_t duration = trajectory.TotalTime(); while (time < duration) { - const Trajectory::State point = trajectory.Sample(time); + const Trajectory::State currentPoint = trajectory.Sample(time); + const Trajectory::State nextPoint = trajectory.Sample(time + dt); time += dt; - const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps, - point.velocity * point.curvature}; - - auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds); - - // Not really a strictly-correct test as we're using the chassis accel - // instead of the wheel accel, but much easier than doing it "properly" and - // a reasonable check anyway - EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) < - maxVoltage + 0.05_V); - EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) > - -maxVoltage - 0.05_V); - EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) < - maxVoltage + 0.05_V); - EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) > - -maxVoltage - 0.05_V); + const ChassisSpeeds currentChassisSpeeds{ + currentPoint.velocity, 0_mps, + currentPoint.velocity * currentPoint.curvature}; + auto [currentLeft, currentRight] = + kinematics.ToWheelSpeeds(currentChassisSpeeds); + + const ChassisSpeeds nextChassisSpeeds{ + nextPoint.velocity, 0_mps, nextPoint.velocity * nextPoint.curvature}; + auto [nextLeft, nextRight] = kinematics.ToWheelSpeeds(nextChassisSpeeds); + + auto leftVoltage = feedforward.Calculate(currentLeft, nextLeft); + auto rightVoltage = feedforward.Calculate(currentRight, nextRight); + + EXPECT_LT(leftVoltage.value(), (maxVoltage + 0.05_V).value()); + EXPECT_GT(leftVoltage.value(), (-maxVoltage - 0.05_V).value()); + EXPECT_LT(rightVoltage.value(), (maxVoltage + 0.05_V).value()); + EXPECT_GT(rightVoltage.value(), (-maxVoltage - 0.05_V).value()); } } TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) { + constexpr units::second_t dt = 20_ms; + SimpleMotorFeedforward feedforward{1_V, 1_V / 1_mps, - 3_V / 1_mps_sq}; + 3_V / 1_mps_sq, dt}; // Large trackwidth - need to test with radius of curvature less than half of // trackwidth const DifferentialDriveKinematics kinematics{3_m}; diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp index 8df3aa1e4b1..663ed3af09d 100644 --- a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp @@ -41,9 +41,9 @@ frc::ExponentialProfile::State CheckDynamics( frc::ExponentialProfile::State current, frc::ExponentialProfile::State goal) { auto next = profile.Calculate(kDt, current, goal); - auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt); + auto signal = feedforward.Calculate(current.velocity, next.velocity); - EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V); + EXPECT_LE(units::math::abs(signal), (constraints.maxInput + 1e-9_V)); return next; } @@ -52,8 +52,8 @@ TEST(ExponentialProfileTest, ReachesGoal) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -69,8 +69,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChange) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -92,8 +92,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -114,8 +114,8 @@ TEST(ExponentialProfileTest, Backwards) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-10_m, 0_mps}; frc::ExponentialProfile::State state; @@ -129,8 +129,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -151,8 +151,8 @@ TEST(ExponentialProfileTest, TopSpeed) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{40_m, 0_mps}; frc::ExponentialProfile::State state; @@ -172,8 +172,8 @@ TEST(ExponentialProfileTest, TopSpeedBackward) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-40_m, 0_mps}; frc::ExponentialProfile::State state; @@ -193,8 +193,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{40_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 8_mps}; @@ -210,8 +210,8 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-40_m, 0_mps}; frc::ExponentialProfile::State state{0_m, -8_mps}; @@ -278,8 +278,8 @@ TEST(ExponentialProfileTest, TimingToCurrent) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{2_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -295,8 +295,8 @@ TEST(ExponentialProfileTest, TimingToGoal) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{2_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -318,8 +318,8 @@ TEST(ExponentialProfileTest, TimingToNegativeGoal) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-2_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps};