[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.controller.HolonomicDriveController;
@@ -17,6 +19,9 @@ import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
 import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
@@ -55,8 +60,22 @@ public class MecanumControllerCommand extends Command {
   private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
   private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
   private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
-  private MecanumDriveWheelSpeeds m_prevSpeeds;
-  private double m_prevTime;
+  private final MutableMeasure<Velocity<Distance>> m_prevFrontLeftSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_prevRearLeftSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_prevFrontRightSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_prevRearRightSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_frontLeftSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_rearLeftSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_frontRightSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_rearRightSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
 
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow the provided
@@ -331,16 +350,20 @@ public class MecanumControllerCommand extends Command {
     var initialYVelocity =
         initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
 
-    m_prevSpeeds =
+    MecanumDriveWheelSpeeds prevSpeeds =
         m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
 
+    m_prevFrontLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontLeftMetersPerSecond);
+    m_prevRearLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearLeftMetersPerSecond);
+    m_prevFrontRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontRightMetersPerSecond);
+    m_prevRearRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearRightMetersPerSecond);
+
     m_timer.restart();
   }
 
   @Override
   public void execute() {
     double curTime = m_timer.get();
-    double dt = curTime - m_prevTime;
 
     var desiredState = m_trajectory.sample(curTime);
 
@@ -350,10 +373,10 @@ public class MecanumControllerCommand extends Command {
 
     targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
 
-    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
-    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
-    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
-    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
+    m_frontLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontLeftMetersPerSecond);
+    m_rearLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearLeftMetersPerSecond);
+    m_frontRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontRightMetersPerSecond);
+    m_rearRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearRightMetersPerSecond);
 
     double frontLeftOutput;
     double rearLeftOutput;
@@ -362,44 +385,42 @@ public class MecanumControllerCommand extends Command {
 
     if (m_usePID) {
       final double frontLeftFeedforward =
-          m_feedforward.calculate(
-              frontLeftSpeedSetpoint,
-              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
+          m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, m_frontLeftSpeedSetpoint).in(Volts);
 
       final double rearLeftFeedforward =
-          m_feedforward.calculate(
-              rearLeftSpeedSetpoint,
-              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
+          m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, m_rearLeftSpeedSetpoint).in(Volts);
 
       final double frontRightFeedforward =
-          m_feedforward.calculate(
-              frontRightSpeedSetpoint,
-              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
+          m_feedforward
+              .calculate(m_prevFrontRightSpeedSetpoint, m_frontRightSpeedSetpoint)
+              .in(Volts);
 
       final double rearRightFeedforward =
-          m_feedforward.calculate(
-              rearRightSpeedSetpoint,
-              (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
+          m_feedforward.calculate(m_prevRearRightSpeedSetpoint, m_rearRightSpeedSetpoint).in(Volts);
 
       frontLeftOutput =
           frontLeftFeedforward
               + m_frontLeftController.calculate(
-                  m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
+                  m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
+                  m_frontLeftSpeedSetpoint.in(MetersPerSecond));
 
       rearLeftOutput =
           rearLeftFeedforward
               + m_rearLeftController.calculate(
-                  m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
+                  m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
+                  m_rearLeftSpeedSetpoint.in(MetersPerSecond));
 
       frontRightOutput =
           frontRightFeedforward
               + m_frontRightController.calculate(
-                  m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
+                  m_currentWheelSpeeds.get().frontRightMetersPerSecond,
+                  m_frontRightSpeedSetpoint.in(MetersPerSecond));
 
       rearRightOutput =
           rearRightFeedforward
               + m_rearRightController.calculate(
-                  m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
+                  m_currentWheelSpeeds.get().rearRightMetersPerSecond,
+                  m_rearRightSpeedSetpoint.in(MetersPerSecond));
 
       m_outputDriveVoltages.accept(
           new MecanumDriveMotorVoltages(
@@ -408,14 +429,11 @@ public class MecanumControllerCommand extends Command {
     } else {
       m_outputWheelSpeeds.accept(
           new MecanumDriveWheelSpeeds(
-              frontLeftSpeedSetpoint,
-              frontRightSpeedSetpoint,
-              rearLeftSpeedSetpoint,
-              rearRightSpeedSetpoint));
+              m_frontLeftSpeedSetpoint.in(MetersPerSecond),
+              m_frontRightSpeedSetpoint.in(MetersPerSecond),
+              m_rearLeftSpeedSetpoint.in(MetersPerSecond),
+              m_rearRightSpeedSetpoint.in(MetersPerSecond)));
     }
-
-    m_prevTime = curTime;
-    m_prevSpeeds = targetWheelSpeeds;
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.controller.PIDController;
@@ -14,6 +16,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Timer;
 import java.util.function.BiConsumer;
@@ -46,6 +51,14 @@ public class RamseteCommand extends Command {
   private final PIDController m_rightController;
   private final BiConsumer<Double, Double> m_output;
   private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds();
+  private final MutableMeasure<Velocity<Distance>> m_prevLeftSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_prevRightSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_leftSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
+  private final MutableMeasure<Velocity<Distance>> m_rightSpeedSetpoint =
+      MutableMeasure.zero(MetersPerSecond);
   private double m_prevTime;
 
   /**
@@ -149,6 +162,8 @@ public class RamseteCommand extends Command {
                 initialState.velocityMetersPerSecond,
                 0,
                 initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond));
+    m_prevLeftSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.leftMetersPerSecond);
+    m_prevRightSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.rightMetersPerSecond);
     m_timer.restart();
     if (m_usePID) {
       m_leftController.reset();
@@ -159,7 +174,6 @@ public class RamseteCommand extends Command {
   @Override
   public void execute() {
     double curTime = m_timer.get();
-    double dt = curTime - m_prevTime;
 
     if (m_prevTime < 0) {
       m_output.accept(0.0, 0.0);
@@ -174,17 +188,18 @@ public class RamseteCommand extends Command {
     var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
     var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
 
+    m_leftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.leftMetersPerSecond);
+    m_rightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rightMetersPerSecond);
+
     double leftOutput;
     double rightOutput;
 
     if (m_usePID) {
       double leftFeedforward =
-          m_feedforward.calculate(
-              leftSpeedSetpoint, (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
+          m_feedforward.calculate(m_prevLeftSpeedSetpoint, m_leftSpeedSetpoint).in(Volts);
 
       double rightFeedforward =
-          m_feedforward.calculate(
-              rightSpeedSetpoint, (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
+          m_feedforward.calculate(m_prevRightSpeedSetpoint, m_rightSpeedSetpoint).in(Volts);
 
       leftOutput =
           leftFeedforward

wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp

@@ -159,7 +159,6 @@ void MecanumControllerCommand::Initialize() {
 
 void MecanumControllerCommand::Execute() {
   auto curTime = m_timer.Get();
-  auto dt = curTime - m_prevTime;
 
   auto m_desiredState = m_trajectory.Sample(curTime);
 
@@ -175,21 +174,17 @@ void MecanumControllerCommand::Execute() {
   auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
 
   if (m_usePID) {
-    auto frontLeftFeedforward = m_feedforward.Calculate(
-        frontLeftSpeedSetpoint,
-        (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt);
+    auto frontLeftFeedforward =
+        m_feedforward.Calculate(m_prevSpeeds.frontLeft, frontLeftSpeedSetpoint);
 
-    auto rearLeftFeedforward = m_feedforward.Calculate(
-        rearLeftSpeedSetpoint,
-        (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt);
+    auto rearLeftFeedforward =
+        m_feedforward.Calculate(m_prevSpeeds.rearLeft, rearLeftSpeedSetpoint);
 
     auto frontRightFeedforward = m_feedforward.Calculate(
-        frontRightSpeedSetpoint,
-        (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt);
+        m_prevSpeeds.frontRight, frontRightSpeedSetpoint);
 
-    auto rearRightFeedforward = m_feedforward.Calculate(
-        rearRightSpeedSetpoint,
-        (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
+    auto rearRightFeedforward =
+        m_feedforward.Calculate(m_prevSpeeds.rearRight, rearRightSpeedSetpoint);
 
     auto frontLeftOutput = units::volt_t{m_frontLeftController->Calculate(
                                m_currentWheelSpeeds().frontLeft.value(),

wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp

@@ -68,7 +68,6 @@ void RamseteCommand::Initialize() {
 
 void RamseteCommand::Execute() {
   auto curTime = m_timer.Get();
-  auto dt = curTime - m_prevTime;
 
   if (m_prevTime < 0_s) {
     if (m_usePID) {
@@ -85,13 +84,11 @@ void RamseteCommand::Execute() {
       m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
 
   if (m_usePID) {
-    auto leftFeedforward = m_feedforward.Calculate(
-        targetWheelSpeeds.left,
-        (targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
+    auto leftFeedforward =
+        m_feedforward.Calculate(m_prevSpeeds.left, targetWheelSpeeds.left);
 
-    auto rightFeedforward = m_feedforward.Calculate(
-        targetWheelSpeeds.right,
-        (targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
+    auto rightFeedforward =
+        m_feedforward.Calculate(m_prevSpeeds.right, targetWheelSpeeds.right);
 
     auto leftOutput =
         units::volt_t{m_leftController->Calculate(