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

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
2026-06-22 01:11:42 +00:00 · 2024-10-11 01:10:45 -04:00
parent 5d9a553104
commit 4adfa8bf64
31 changed files with 1004 additions and 425 deletions
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp
@@ -48,7 +48,7 @@ void Elevator::ReachGoal(units::meter_t goal) {
  auto pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
                                          m_setpoint.position / 1_m);
  auto feedforwardOutput =
-      m_feedforward.Calculate(m_setpoint.velocity, next.velocity, 20_ms);
+      m_feedforward.Calculate(m_setpoint.velocity, next.velocity);

  m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);

--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -7,6 +7,7 @@
 #include <numbers>

 #include <units/angle.h>
+#include <units/angular_velocity.h>
 #include <units/time.h>
 #include <units/voltage.h>

--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -28,5 +28,5 @@ class ShooterSubsystem : public frc2::PIDSubsystem {
  frc::PWMSparkMax m_shooterMotor;
  frc::PWMSparkMax m_feederMotor;
  frc::Encoder m_shooterEncoder;
-  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
+  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward;
 };
--- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h
+++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h
@@ -38,7 +38,7 @@ class Shooter : public frc2::SubsystemBase {
  frc::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0],
                                ShooterConstants::kEncoderPorts[1],
                                ShooterConstants::kEncoderReversed};
-  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward{
      ShooterConstants::kS, ShooterConstants::kV};
  frc::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
 };
--- a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
@@ -60,7 +60,7 @@ inline constexpr units::turns_per_second_t kShooterTolerance = 50_tps;
 inline constexpr double kP = 1.0;

 inline constexpr units::volt_t kS = 0.05_V;
-inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+inline constexpr kv_unit_t kV = 12_V / kShooterFreeSpeed;
 inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / 1_tr;

 inline constexpr double kFeederSpeed = 0.5;
--- a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h
@@ -48,6 +48,6 @@ class Shooter : public frc2::SubsystemBase {
          },
          this}};
  frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0};
-  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward{
      constants::shooter::kS, constants::shooter::kV, constants::shooter::kA};
 };
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
@@ -4,6 +4,10 @@

 package edu.wpi.first.wpilibj.examples.armbot.subsystems;

+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -41,7 +45,10 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
  @Override
  public void useOutput(double output, TrapezoidProfile.State setpoint) {
    // Calculate the feedforward from the sepoint
-    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+    double feedforward =
+        m_feedforward
+            .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
+            .in(Volts);
    // Add the feedforward to the PID output to get the motor output
    m_motor.setVoltage(output + feedforward);
  }
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
@@ -4,6 +4,10 @@

 package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;

+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
@@ -33,7 +37,10 @@ public class ArmSubsystem extends TrapezoidProfileSubsystem {
  @Override
  public void useState(TrapezoidProfile.State setpoint) {
    // Calculate the feedforward from the sepoint
-    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+    double feedforward =
+        m_feedforward
+            .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
+            .in(Volts);
    // Add the feedforward to the PID output to get the motor output
    m_motor.setSetpoint(
        ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java
@@ -4,6 +4,9 @@

 package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems;

+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -110,7 +113,10 @@ public class Elevator implements AutoCloseable {

    // With the setpoint value we run PID control like normal
    double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position);
-    double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.020);
+    double feedforwardOutput =
+        m_feedforward
+            .calculate(MetersPerSecond.of(m_setpoint.velocity), MetersPerSecond.of(next.velocity))
+            .in(Volts);

    m_motor.setVoltage(pidOutput + feedforwardOutput);

--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -4,6 +4,9 @@

 package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;

+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -51,6 +54,8 @@ public class Robot extends TimedRobot {
    // Run controller and update motor output
    m_motor.setVoltage(
        m_controller.calculate(m_encoder.getDistance())
-            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
+            + m_feedforward
+                .calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity))
+                .in(Volts));
  }
 }
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java
@@ -4,6 +4,9 @@

 package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems;

+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -101,7 +104,8 @@ public class Elevator implements AutoCloseable {

    // With the setpoint value we run PID control like normal
    double pidOutput = m_controller.calculate(m_encoder.getDistance());
-    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
+    double feedforwardOutput =
+        m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts);
    m_motor.setVoltage(pidOutput + feedforwardOutput);
  }

--- a/wpimath/algorithms.md
+++ b/wpimath/algorithms.md
@@ -1,5 +1,219 @@
 # Algorithms

+## Simple motor feedforward
+
+### Derivation
+
+For a simple DC motor with the model
+
+```
+  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kₛ/kₐ sgn(x),
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  c = -kₛ/kₐ sgn(x)
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+  dx/dt = Ax + Bu + c
+```
+
+Discretize the affine model.
+
+```
+  dx/dt = Ax + Bu + c
+  dx/dt = Ax + B(u + B⁺c)
+  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
+```
+
+Solve for uₖ.
+
+```
+  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+```
+
+Substitute in B assuming sgn(x) is a constant for the duration of the step.
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kₛ/kₐ sgn(x)))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kₛ/kₐ sgn(x))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
+```
+
+Simplify the 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⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁) + kₛ sgn(x)
+  uₖ = kᵥxₖ₊₁ + kₛ sgn(x)
+  uₖ = kₛ sgn(x) + kᵥxₖ₊₁
+```
+
+Simplify the model when ka ≠ 0
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+```
+
+## Elevator feedforward
+
+### Derivation
+
+For an elevator with the model
+
+```
+  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kg/kₐ - kₛ/kₐ sgn(x)
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  c = -(kg/kₐ + kₛ/kₐ sgn(x))
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+  dx/dt = Ax + Bu + c
+```
+
+Discretize the affine model.
+
+```
+  dx/dt = Ax + Bu + c
+  dx/dt = Ax + B(u + B⁺c)
+  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
+```
+
+Solve for uₖ.
+
+```
+  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+```
+
+Substitute in B assuming sgn(x) is a constant for the duration of the step.
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kg/kₐ + kₛ/kₐ sgn(x)))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kg/kₐ + kₛ/kₐ sgn(x))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
+```
+
+Simplify the 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⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kg + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁) + kg + kₛ sgn(x)
+  uₖ = kᵥxₖ₊₁ + kg + kₛ sgn(x)
+  uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁
+```
+
+Simplify the model when ka ≠ 0
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+```
+
 ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I

 ### Derivation
--- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java
+++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java
@@ -18,49 +18,51 @@ import us.hebi.quickbuf.ProtoUtil;
 import us.hebi.quickbuf.RepeatedByte;

 public final class Controller {
-  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1755,
-    "ChBjb250cm9sbGVyLnByb3RvEgl3cGkucHJvdG8iWAoWUHJvdG9idWZBcm1GZWVkZm9yd2FyZBIOCgJr" + 
-    "cxgBIAEoAVICa3MSDgoCa2cYAiABKAFSAmtnEg4KAmt2GAMgASgBUgJrdhIOCgJrYRgEIAEoAVICa2Ei" + 
-    "ngEKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVGZWVkZm9yd2FyZBIbCglrdl9saW5lYXIYASABKAFS" + 
-    "CGt2TGluZWFyEhsKCWthX2xpbmVhchgCIAEoAVIIa2FMaW5lYXISHQoKa3ZfYW5ndWxhchgDIAEoAVIJ" + 
-    "a3ZBbmd1bGFyEh0KCmthX2FuZ3VsYXIYBCABKAFSCWthQW5ndWxhciJdChtQcm90b2J1ZkVsZXZhdG9y" + 
-    "RmVlZGZvcndhcmQSDgoCa3MYASABKAFSAmtzEg4KAmtnGAIgASgBUgJrZxIOCgJrdhgDIAEoAVICa3YS" + 
-    "DgoCa2EYBCABKAFSAmthImAKHlByb3RvYnVmU2ltcGxlTW90b3JGZWVkZm9yd2FyZBIOCgJrcxgBIAEo" + 
-    "AVICa3MSDgoCa3YYAiABKAFSAmt2Eg4KAmthGAMgASgBUgJrYRIOCgJkdBgEIAEoAVICZHQiUgomUHJv" + 
-    "dG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsVm9sdGFnZXMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVy" + 
-    "aWdodBgCIAEoAVIFcmlnaHRCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3RvSocJCgYSBAAAJQEKCAoB" + 
-    "DBIDAAASCggKAQISAwIAEgoICgEIEgMEADEKCQoCCAESAwQAMQoKCgIEABIEBgALAQoKCgMEAAESAwYI" + 
-    "HgoLCgQEAAIAEgMHAhAKDAoFBAACAAUSAwcCCAoMCgUEAAIAARIDBwkLCgwKBQQAAgADEgMHDg8KCwoE" + 
-    "BAACARIDCAIQCgwKBQQAAgEFEgMIAggKDAoFBAACAQESAwgJCwoMCgUEAAIBAxIDCA4PCgsKBAQAAgIS" + 
-    "AwkCEAoMCgUEAAICBRIDCQIICgwKBQQAAgIBEgMJCQsKDAoFBAACAgMSAwkODwoLCgQEAAIDEgMKAhAK" + 
-    "DAoFBAACAwUSAwoCCAoMCgUEAAIDARIDCgkLCgwKBQQAAgMDEgMKDg8KCgoCBAESBA0AEgEKCgoDBAEB" + 
-    "EgMNCCwKCwoEBAECABIDDgIXCgwKBQQBAgAFEgMOAggKDAoFBAECAAESAw4JEgoMCgUEAQIAAxIDDhUW" + 
-    "CgsKBAQBAgESAw8CFwoMCgUEAQIBBRIDDwIICgwKBQQBAgEBEgMPCRIKDAoFBAECAQMSAw8VFgoLCgQE" + 
-    "AQICEgMQAhgKDAoFBAECAgUSAxACCAoMCgUEAQICARIDEAkTCgwKBQQBAgIDEgMQFhcKCwoEBAECAxID" + 
-    "EQIYCgwKBQQBAgMFEgMRAggKDAoFBAECAwESAxEJEwoMCgUEAQIDAxIDERYXCgoKAgQCEgQUABkBCgoK" + 
-    "AwQCARIDFAgjCgsKBAQCAgASAxUCEAoMCgUEAgIABRIDFQIICgwKBQQCAgABEgMVCQsKDAoFBAICAAMS",
-    "AxUODwoLCgQEAgIBEgMWAhAKDAoFBAICAQUSAxYCCAoMCgUEAgIBARIDFgkLCgwKBQQCAgEDEgMWDg8K" + 
-    "CwoEBAICAhIDFwIQCgwKBQQCAgIFEgMXAggKDAoFBAICAgESAxcJCwoMCgUEAgICAxIDFw4PCgsKBAQC" + 
-    "AgMSAxgCEAoMCgUEAgIDBRIDGAIICgwKBQQCAgMBEgMYCQsKDAoFBAICAwMSAxgODwoKCgIEAxIEGwAg" + 
-    "AQoKCgMEAwESAxsIJgoLCgQEAwIAEgMcAhAKDAoFBAMCAAUSAxwCCAoMCgUEAwIAARIDHAkLCgwKBQQD" + 
-    "AgADEgMcDg8KCwoEBAMCARIDHQIQCgwKBQQDAgEFEgMdAggKDAoFBAMCAQESAx0JCwoMCgUEAwIBAxID" + 
-    "HQ4PCgsKBAQDAgISAx4CEAoMCgUEAwICBRIDHgIICgwKBQQDAgIBEgMeCQsKDAoFBAMCAgMSAx4ODwoL" + 
-    "CgQEAwIDEgMfAhAKDAoFBAMCAwUSAx8CCAoMCgUEAwIDARIDHwkLCgwKBQQDAgMDEgMfDg8KCgoCBAQS" + 
-    "BCIAJQEKCgoDBAQBEgMiCC4KCwoEBAQCABIDIwISCgwKBQQEAgAFEgMjAggKDAoFBAQCAAESAyMJDQoM" + 
-    "CgUEBAIAAxIDIxARCgsKBAQEAgESAyQCEwoMCgUEBAIBBRIDJAIICgwKBQQEAgEBEgMkCQ4KDAoFBAQC" + 
-    "AQMSAyQREmIGcHJvdG8z");
+  private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1897,
+    "ChBjb250cm9sbGVyLnByb3RvEgl3cGkucHJvdG8iaAoWUHJvdG9idWZBcm1GZWVkZm9yd2FyZBIOCgJr" + 
+    "cxgBIAEoAVICa3MSDgoCa2cYAiABKAFSAmtnEg4KAmt2GAMgASgBUgJrdhIOCgJrYRgEIAEoAVICa2ES" + 
+    "DgoCZHQYBSABKAFSAmR0Ip4BCiRQcm90b2J1ZkRpZmZlcmVudGlhbERyaXZlRmVlZGZvcndhcmQSGwoJ" + 
+    "a3ZfbGluZWFyGAEgASgBUghrdkxpbmVhchIbCglrYV9saW5lYXIYAiABKAFSCGthTGluZWFyEh0KCmt2" + 
+    "X2FuZ3VsYXIYAyABKAFSCWt2QW5ndWxhchIdCgprYV9hbmd1bGFyGAQgASgBUglrYUFuZ3VsYXIibQob" + 
+    "UHJvdG9idWZFbGV2YXRvckZlZWRmb3J3YXJkEg4KAmtzGAEgASgBUgJrcxIOCgJrZxgCIAEoAVICa2cS" + 
+    "DgoCa3YYAyABKAFSAmt2Eg4KAmthGAQgASgBUgJrYRIOCgJkdBgFIAEoAVICZHQiYAoeUHJvdG9idWZT" + 
+    "aW1wbGVNb3RvckZlZWRmb3J3YXJkEg4KAmtzGAEgASgBUgJrcxIOCgJrdhgCIAEoAVICa3YSDgoCa2EY" + 
+    "AyABKAFSAmthEg4KAmR0GAQgASgBUgJkdCJSCiZQcm90b2J1ZkRpZmZlcmVudGlhbERyaXZlV2hlZWxW" + 
+    "b2x0YWdlcxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodEIaChhlZHUud3Bp" + 
+    "LmZpcnN0Lm1hdGgucHJvdG9K9QkKBhIEAAAnAQoICgEMEgMAABIKCAoBAhIDAgASCggKAQgSAwQAMQoJ" + 
+    "CgIIARIDBAAxCgoKAgQAEgQGAAwBCgoKAwQAARIDBggeCgsKBAQAAgASAwcCEAoMCgUEAAIABRIDBwII" + 
+    "CgwKBQQAAgABEgMHCQsKDAoFBAACAAMSAwcODwoLCgQEAAIBEgMIAhAKDAoFBAACAQUSAwgCCAoMCgUE" + 
+    "AAIBARIDCAkLCgwKBQQAAgEDEgMIDg8KCwoEBAACAhIDCQIQCgwKBQQAAgIFEgMJAggKDAoFBAACAgES" + 
+    "AwkJCwoMCgUEAAICAxIDCQ4PCgsKBAQAAgMSAwoCEAoMCgUEAAIDBRIDCgIICgwKBQQAAgMBEgMKCQsK" + 
+    "DAoFBAACAwMSAwoODwoLCgQEAAIEEgMLAhAKDAoFBAACBAUSAwsCCAoMCgUEAAIEARIDCwkLCgwKBQQA" + 
+    "AgQDEgMLDg8KCgoCBAESBA4AEwEKCgoDBAEBEgMOCCwKCwoEBAECABIDDwIXCgwKBQQBAgAFEgMPAggK" + 
+    "DAoFBAECAAESAw8JEgoMCgUEAQIAAxIDDxUWCgsKBAQBAgESAxACFwoMCgUEAQIBBRIDEAIICgwKBQQB" + 
+    "AgEBEgMQCRIKDAoFBAECAQMSAxAVFgoLCgQEAQICEgMRAhgKDAoFBAECAgUSAxECCAoMCgUEAQICARID" + 
+    "EQkTCgwKBQQBAgIDEgMRFhcKCwoEBAECAxIDEgIYCgwKBQQBAgMFEgMSAggKDAoFBAECAwESAxIJEwoM",
+    "CgUEAQIDAxIDEhYXCgoKAgQCEgQVABsBCgoKAwQCARIDFQgjCgsKBAQCAgASAxYCEAoMCgUEAgIABRID" + 
+    "FgIICgwKBQQCAgABEgMWCQsKDAoFBAICAAMSAxYODwoLCgQEAgIBEgMXAhAKDAoFBAICAQUSAxcCCAoM" + 
+    "CgUEAgIBARIDFwkLCgwKBQQCAgEDEgMXDg8KCwoEBAICAhIDGAIQCgwKBQQCAgIFEgMYAggKDAoFBAIC" + 
+    "AgESAxgJCwoMCgUEAgICAxIDGA4PCgsKBAQCAgMSAxkCEAoMCgUEAgIDBRIDGQIICgwKBQQCAgMBEgMZ" + 
+    "CQsKDAoFBAICAwMSAxkODwoLCgQEAgIEEgMaAhAKDAoFBAICBAUSAxoCCAoMCgUEAgIEARIDGgkLCgwK" + 
+    "BQQCAgQDEgMaDg8KCgoCBAMSBB0AIgEKCgoDBAMBEgMdCCYKCwoEBAMCABIDHgIQCgwKBQQDAgAFEgMe" + 
+    "AggKDAoFBAMCAAESAx4JCwoMCgUEAwIAAxIDHg4PCgsKBAQDAgESAx8CEAoMCgUEAwIBBRIDHwIICgwK" + 
+    "BQQDAgEBEgMfCQsKDAoFBAMCAQMSAx8ODwoLCgQEAwICEgMgAhAKDAoFBAMCAgUSAyACCAoMCgUEAwIC" + 
+    "ARIDIAkLCgwKBQQDAgIDEgMgDg8KCwoEBAMCAxIDIQIQCgwKBQQDAgMFEgMhAggKDAoFBAMCAwESAyEJ" + 
+    "CwoMCgUEAwIDAxIDIQ4PCgoKAgQEEgQkACcBCgoKAwQEARIDJAguCgsKBAQEAgASAyUCEgoMCgUEBAIA" + 
+    "BRIDJQIICgwKBQQEAgABEgMlCQ0KDAoFBAQCAAMSAyUQEQoLCgQEBAIBEgMmAhMKDAoFBAQCAQUSAyYC" + 
+    "CAoMCgUEBAIBARIDJgkOCgwKBQQEAgEDEgMmERJiBnByb3RvMw==");

  static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("controller.proto", "wpi.proto", descriptorData);

-  static final Descriptors.Descriptor wpi_proto_ProtobufArmFeedforward_descriptor = descriptor.internalContainedType(31, 88, "ProtobufArmFeedforward", "wpi.proto.ProtobufArmFeedforward");
+  static final Descriptors.Descriptor wpi_proto_ProtobufArmFeedforward_descriptor = descriptor.internalContainedType(31, 104, "ProtobufArmFeedforward", "wpi.proto.ProtobufArmFeedforward");

-  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveFeedforward_descriptor = descriptor.internalContainedType(122, 158, "ProtobufDifferentialDriveFeedforward", "wpi.proto.ProtobufDifferentialDriveFeedforward");
+  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveFeedforward_descriptor = descriptor.internalContainedType(138, 158, "ProtobufDifferentialDriveFeedforward", "wpi.proto.ProtobufDifferentialDriveFeedforward");

-  static final Descriptors.Descriptor wpi_proto_ProtobufElevatorFeedforward_descriptor = descriptor.internalContainedType(282, 93, "ProtobufElevatorFeedforward", "wpi.proto.ProtobufElevatorFeedforward");
+  static final Descriptors.Descriptor wpi_proto_ProtobufElevatorFeedforward_descriptor = descriptor.internalContainedType(298, 109, "ProtobufElevatorFeedforward", "wpi.proto.ProtobufElevatorFeedforward");

-  static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(377, 96, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward");
+  static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(409, 96, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward");

-  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(475, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages");
+  static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(507, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages");

  /**
   * @return this proto file's descriptor.
@@ -95,6 +97,11 @@ public final class Controller {
     */
    private double ka;

+    /**
+     * <code>optional double dt = 5;</code>
+     */
+    private double dt;
+
    private ProtobufArmFeedforward() {
    }

@@ -253,6 +260,43 @@ public final class Controller {
      return this;
    }

+    /**
+     * <code>optional double dt = 5;</code>
+     * @return whether the dt field is set
+     */
+    public boolean hasDt() {
+      return (bitField0_ & 0x00000010) != 0;
+    }
+
+    /**
+     * <code>optional double dt = 5;</code>
+     * @return this
+     */
+    public ProtobufArmFeedforward clearDt() {
+      bitField0_ &= ~0x00000010;
+      dt = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dt = 5;</code>
+     * @return the dt
+     */
+    public double getDt() {
+      return dt;
+    }
+
+    /**
+     * <code>optional double dt = 5;</code>
+     * @param value the dt to set
+     * @return this
+     */
+    public ProtobufArmFeedforward setDt(final double value) {
+      bitField0_ |= 0x00000010;
+      dt = value;
+      return this;
+    }
+
    @Override
    public ProtobufArmFeedforward copyFrom(final ProtobufArmFeedforward other) {
      cachedSize = other.cachedSize;
@@ -262,6 +306,7 @@ public final class Controller {
        kg = other.kg;
        kv = other.kv;
        ka = other.ka;
+        dt = other.dt;
      }
      return this;
    }
@@ -284,6 +329,9 @@ public final class Controller {
      if (other.hasKa()) {
        setKa(other.ka);
      }
+      if (other.hasDt()) {
+        setDt(other.dt);
+      }
      return this;
    }

@@ -298,6 +346,7 @@ public final class Controller {
      kg = 0D;
      kv = 0D;
      ka = 0D;
+      dt = 0D;
      return this;
    }

@@ -324,7 +373,8 @@ public final class Controller {
        && (!hasKs() || ProtoUtil.isEqual(ks, other.ks))
        && (!hasKg() || ProtoUtil.isEqual(kg, other.kg))
        && (!hasKv() || ProtoUtil.isEqual(kv, other.kv))
-        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka));
+        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka))
+        && (!hasDt() || ProtoUtil.isEqual(dt, other.dt));
    }

    @Override
@@ -345,6 +395,10 @@ public final class Controller {
        output.writeRawByte((byte) 33);
        output.writeDoubleNoTag(ka);
      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeRawByte((byte) 41);
+        output.writeDoubleNoTag(dt);
+      }
    }

    @Override
@@ -362,6 +416,9 @@ public final class Controller {
      if ((bitField0_ & 0x00000008) != 0) {
        size += 9;
      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        size += 9;
+      }
      return size;
    }

@@ -404,6 +461,15 @@ public final class Controller {
            ka = input.readDouble();
            bitField0_ |= 0x00000008;
            tag = input.readTag();
+            if (tag != 41) {
+              break;
+            }
+          }
+          case 41: {
+            // dt
+            dt = input.readDouble();
+            bitField0_ |= 0x00000010;
+            tag = input.readTag();
            if (tag != 0) {
              break;
            }
@@ -437,6 +503,9 @@ public final class Controller {
      if ((bitField0_ & 0x00000008) != 0) {
        output.writeDouble(FieldNames.ka, ka);
      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeDouble(FieldNames.dt, dt);
+      }
      output.endObject();
    }

@@ -491,6 +560,17 @@ public final class Controller {
            }
            break;
          }
+          case 3216: {
+            if (input.isAtField(FieldNames.dt)) {
+              if (!input.trySkipNullValue()) {
+                dt = input.readDouble();
+                bitField0_ |= 0x00000010;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
          default: {
            input.skipUnknownField();
            break;
@@ -558,6 +638,8 @@ public final class Controller {
      static final FieldName kv = FieldName.forField("kv");

      static final FieldName ka = FieldName.forField("ka");
+
+      static final FieldName dt = FieldName.forField("dt");
    }
  }

@@ -1089,6 +1171,11 @@ public final class Controller {
     */
    private double ka;

+    /**
+     * <code>optional double dt = 5;</code>
+     */
+    private double dt;
+
    private ProtobufElevatorFeedforward() {
    }

@@ -1247,6 +1334,43 @@ public final class Controller {
      return this;
    }

+    /**
+     * <code>optional double dt = 5;</code>
+     * @return whether the dt field is set
+     */
+    public boolean hasDt() {
+      return (bitField0_ & 0x00000010) != 0;
+    }
+
+    /**
+     * <code>optional double dt = 5;</code>
+     * @return this
+     */
+    public ProtobufElevatorFeedforward clearDt() {
+      bitField0_ &= ~0x00000010;
+      dt = 0D;
+      return this;
+    }
+
+    /**
+     * <code>optional double dt = 5;</code>
+     * @return the dt
+     */
+    public double getDt() {
+      return dt;
+    }
+
+    /**
+     * <code>optional double dt = 5;</code>
+     * @param value the dt to set
+     * @return this
+     */
+    public ProtobufElevatorFeedforward setDt(final double value) {
+      bitField0_ |= 0x00000010;
+      dt = value;
+      return this;
+    }
+
    @Override
    public ProtobufElevatorFeedforward copyFrom(final ProtobufElevatorFeedforward other) {
      cachedSize = other.cachedSize;
@@ -1256,6 +1380,7 @@ public final class Controller {
        kg = other.kg;
        kv = other.kv;
        ka = other.ka;
+        dt = other.dt;
      }
      return this;
    }
@@ -1278,6 +1403,9 @@ public final class Controller {
      if (other.hasKa()) {
        setKa(other.ka);
      }
+      if (other.hasDt()) {
+        setDt(other.dt);
+      }
      return this;
    }

@@ -1292,6 +1420,7 @@ public final class Controller {
      kg = 0D;
      kv = 0D;
      ka = 0D;
+      dt = 0D;
      return this;
    }

@@ -1318,7 +1447,8 @@ public final class Controller {
        && (!hasKs() || ProtoUtil.isEqual(ks, other.ks))
        && (!hasKg() || ProtoUtil.isEqual(kg, other.kg))
        && (!hasKv() || ProtoUtil.isEqual(kv, other.kv))
-        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka));
+        && (!hasKa() || ProtoUtil.isEqual(ka, other.ka))
+        && (!hasDt() || ProtoUtil.isEqual(dt, other.dt));
    }

    @Override
@@ -1339,6 +1469,10 @@ public final class Controller {
        output.writeRawByte((byte) 33);
        output.writeDoubleNoTag(ka);
      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeRawByte((byte) 41);
+        output.writeDoubleNoTag(dt);
+      }
    }

    @Override
@@ -1356,6 +1490,9 @@ public final class Controller {
      if ((bitField0_ & 0x00000008) != 0) {
        size += 9;
      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        size += 9;
+      }
      return size;
    }

@@ -1398,6 +1535,15 @@ public final class Controller {
            ka = input.readDouble();
            bitField0_ |= 0x00000008;
            tag = input.readTag();
+            if (tag != 41) {
+              break;
+            }
+          }
+          case 41: {
+            // dt
+            dt = input.readDouble();
+            bitField0_ |= 0x00000010;
+            tag = input.readTag();
            if (tag != 0) {
              break;
            }
@@ -1431,6 +1577,9 @@ public final class Controller {
      if ((bitField0_ & 0x00000008) != 0) {
        output.writeDouble(FieldNames.ka, ka);
      }
+      if ((bitField0_ & 0x00000010) != 0) {
+        output.writeDouble(FieldNames.dt, dt);
+      }
      output.endObject();
    }

@@ -1485,6 +1634,17 @@ public final class Controller {
            }
            break;
          }
+          case 3216: {
+            if (input.isAtField(FieldNames.dt)) {
+              if (!input.trySkipNullValue()) {
+                dt = input.readDouble();
+                bitField0_ |= 0x00000010;
+              }
+            } else {
+              input.skipUnknownField();
+            }
+            break;
+          }
          default: {
            input.skipUnknownField();
            break;
@@ -1553,6 +1713,8 @@ public final class Controller {
      static final FieldName kv = FieldName.forField("kv");

      static final FieldName ka = FieldName.forField("ka");
+
+      static final FieldName dt = FieldName.forField("dt");
    }
  }

--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -4,9 +4,17 @@

 package edu.wpi.first.math.controller;

+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
 import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
 import edu.wpi.first.math.jni.ArmFeedforwardJNI;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;

@@ -27,24 +35,25 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
  /** The acceleration gain, in V/(rad/s²). */
  private final double ka;

-  /** Arm feedforward protobuf for serialization. */
-  public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
+  /** The period, in seconds. */
+  private final double m_dt;

-  /** Arm feedforward struct for serialization. */
-  public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
+  /** The calculated output voltage measure. */
+  private final MutVoltage output = Volts.mutable(0.0);

  /**
-   * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate
-   * units of the computed feedforward.
+   * Creates a new ArmFeedforward with the specified gains and period.
   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(rad/s).
+   * @param ka The acceleration gain in V/(rad/s²).
+   * @param dtSeconds The period in seconds.
   * @throws IllegalArgumentException for kv &lt; zero.
   * @throws IllegalArgumentException for ka &lt; zero.
+   * @throws IllegalArgumentException for period &le; zero.
   */
-  public ArmFeedforward(double ks, double kg, double kv, double ka) {
+  public ArmFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
    this.ks = ks;
    this.kg = kg;
    this.kv = kv;
@@ -55,56 +64,84 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
    if (ka < 0.0) {
      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
    }
+    if (dtSeconds <= 0.0) {
+      throw new IllegalArgumentException(
+          "period must be a positive number, got " + dtSeconds + "!");
+    }
+    m_dt = dtSeconds;
  }

  /**
-   * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero.
-   * Units of the gain values will dictate units of the computed feedforward.
+   * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms.
   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(rad/s).
+   * @param ka The acceleration gain in V/(rad/s²).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   */
+  public ArmFeedforward(double ks, double kg, double kv, double ka) {
+    this(ks, kg, kv, ka, 0.020);
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(rad/s).
+   * @throws IllegalArgumentException for kv &lt; zero.
   */
  public ArmFeedforward(double ks, double kg, double kv) {
    this(ks, kg, kv, 0);
  }

  /**
-   * Returns the static gain.
+   * Returns the static gain in volts.
   *
-   * @return The static gain, in volts.
+   * @return The static gain in volts.
   */
  public double getKs() {
    return ks;
  }

  /**
-   * Returns the gravity gain.
+   * Returns the gravity gain in volts.
   *
-   * @return The gravity gain, in volts.
+   * @return The gravity gain in volts.
   */
  public double getKg() {
    return kg;
  }

  /**
-   * Returns the velocity gain.
+   * Returns the velocity gain in V/(rad/s).
   *
-   * @return The velocity gain, in V/(rad/s).
+   * @return The velocity gain.
   */
  public double getKv() {
    return kv;
  }

  /**
-   * Returns the acceleration gain.
+   * Returns the acceleration gain in V/(rad/s²).
   *
-   * @return The acceleration gain, in V/(rad/s²).
+   * @return The acceleration gain.
   */
  public double getKa() {
    return ka;
  }

+  /**
+   * Returns the period in seconds.
+   *
+   * @return The period in seconds.
+   */
+  public double getDt() {
+    return m_dt;
+  }
+
  /**
   * Calculates the feedforward from the gains and setpoints.
   *
@@ -115,6 +152,7 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * @param accelRadPerSecSquared The acceleration setpoint.
   * @return The computed feedforward.
   */
+  @Deprecated(forRemoval = true, since = "2025")
  public double calculate(
      double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
    return ks * Math.signum(velocityRadPerSec)
@@ -124,8 +162,8 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
  }

  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
-   * zero).
+   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
+   * (acceleration is assumed to be zero).
   *
   * @param positionRadians The position (angle) setpoint. This angle should be measured from the
   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
@@ -133,12 +171,14 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * @param velocity The velocity setpoint.
   * @return The computed feedforward.
   */
+  @SuppressWarnings("removal")
+  @Deprecated(forRemoval = true, since = "2025")
  public double calculate(double positionRadians, double velocity) {
    return calculate(positionRadians, velocity, 0);
  }

  /**
-   * 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
@@ -148,12 +188,59 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * @param dt Time between velocity setpoints in seconds.
   * @return The computed feedforward in volts.
   */
+  @SuppressWarnings("removal")
+  @Deprecated(forRemoval = true, since = "2025")
  public double calculate(
      double currentAngle, double currentVelocity, double nextVelocity, double dt) {
    return ArmFeedforwardJNI.calculate(
        ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
  }

+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete control 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.
+   */
+  public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) {
+    output.mut_replace(
+        kg * Math.cos(currentAngle.in(Radians))
+            + ks * Math.signum(currentVelocity.in(RadiansPerSecond))
+            + kv * currentVelocity.in(RadiansPerSecond),
+        Volts);
+    return output;
+  }
+
+  /**
+   * 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 setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @return The computed feedforward in volts.
+   */
+  public Voltage calculate(
+      Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
+    output.mut_replace(
+        ArmFeedforwardJNI.calculate(
+            ks,
+            kv,
+            ka,
+            kg,
+            currentAngle.in(Radians),
+            currentVelocity.in(RadiansPerSecond),
+            nextVelocity.in(RadiansPerSecond),
+            m_dt),
+        Volts);
+    return output;
+  }
+
  // Rearranging the main equation from the calculate() method yields the
  // formulas for the methods below:

@@ -164,11 +251,11 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * you a simultaneously-achievable velocity constraint.
   *
   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
-   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
-   *     not follow this convention, an offset should be added.
-   * @param acceleration The acceleration of the arm.
-   * @return The maximum possible velocity at the given acceleration and angle.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param acceleration The acceleration of the arm, in (rad/s²).
+   * @return The maximum possible velocity in (rad/s) at the given acceleration and angle.
   */
  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
    // Assume max velocity is positive
@@ -181,12 +268,12 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
   * you a simultaneously-achievable velocity constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
-   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
-   *     not follow this convention, an offset should be added.
-   * @param acceleration The acceleration of the arm.
-   * @return The minimum possible velocity at the given acceleration and angle.
+   * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param acceleration The acceleration of the arm, in (rad/s²).
+   * @return The minimum possible velocity in (rad/s) at the given acceleration and angle.
   */
  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
    // Assume min velocity is negative, ks flips sign
@@ -199,12 +286,12 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
   * simultaneously-achievable acceleration constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
-   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
-   *     not follow this convention, an offset should be added.
-   * @param velocity The velocity of the arm.
-   * @return The maximum possible acceleration at the given velocity.
+   * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param velocity The velocity of the elevator, in (rad/s)
+   * @return The maximum possible acceleration in (rad/s²) at the given velocity.
   */
  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
@@ -216,14 +303,20 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
   * simultaneously-achievable acceleration constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
-   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
-   *     not follow this convention, an offset should be added.
-   * @param velocity The velocity of the arm.
-   * @return The minimum possible acceleration at the given velocity.
+   * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param velocity The velocity of the elevator, in (rad/s)
+   * @return The maximum possible acceleration in (rad/s²) at the given velocity.
   */
  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
  }
+
+  /** Arm feedforward struct for serialization. */
+  public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
+
+  /** Arm feedforward protobuf for serialization. */
+  public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -4,11 +4,14 @@

 package edu.wpi.first.math.controller;

-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
 import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
-import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;

@@ -17,36 +20,37 @@ import edu.wpi.first.util.struct.StructSerializable;
 * against the force of gravity).
 */
 public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
-  /** The static gain. */
+  /** The static gain, in volts. */
  private final double ks;

-  /** The gravity gain. */
+  /** The gravity gain, in volts. */
  private final double kg;

-  /** The velocity gain. */
+  /** The velocity gain, in V/(m/s). */
  private final double kv;

-  /** The acceleration gain. */
+  /** The acceleration gain, in V/(m/s²). */
  private final double ka;

-  /** ElevatorFeedforward protobuf for serialization. */
-  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
+  /** The period, in seconds. */
+  private final double m_dt;

-  /** ElevatorFeedforward struct for serialization. */
-  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
+  /** The calculated output voltage measure. */
+  private final MutVoltage output = Volts.mutable(0.0);

  /**
-   * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
-   * dictate units of the computed feedforward.
+   * Creates a new ElevatorFeedforward with the specified gains and period.
   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(m/s).
+   * @param ka The acceleration gain in V/(m/s²).
+   * @param dtSeconds The period in seconds.
   * @throws IllegalArgumentException for kv &lt; zero.
   * @throws IllegalArgumentException for ka &lt; zero.
+   * @throws IllegalArgumentException for period &le; zero.
   */
-  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
    this.ks = ks;
    this.kg = kg;
    this.kv = kv;
@@ -57,40 +61,60 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
    if (ka < 0.0) {
      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
    }
+    if (dtSeconds <= 0.0) {
+      throw new IllegalArgumentException(
+          "period must be a positive number, got " + dtSeconds + "!");
+    }
+    m_dt = dtSeconds;
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(m/s).
+   * @param ka The acceleration gain in V/(m/s²).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+    this(ks, kg, kv, ka, 0.020);
  }

  /**
   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
-   * zero. Units of the gain values will dictate units of the computed feedforward.
+   * zero. The period is defaulted to 20 ms.
   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(m/s).
+   * @throws IllegalArgumentException for kv &lt; zero.
   */
  public ElevatorFeedforward(double ks, double kg, double kv) {
    this(ks, kg, kv, 0);
  }

  /**
-   * Returns the static gain.
+   * Returns the static gain in volts.
   *
-   * @return The static gain.
+   * @return The static gain in volts.
   */
  public double getKs() {
    return ks;
  }

  /**
-   * Returns the gravity gain.
+   * Returns the gravity gain in volts.
   *
-   * @return The gravity gain.
+   * @return The gravity gain in volts.
   */
  public double getKg() {
    return kg;
  }

  /**
-   * Returns the velocity gain.
+   * Returns the velocity gain in V/(m/s).
   *
   * @return The velocity gain.
   */
@@ -99,7 +123,7 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
  }

  /**
-   * Returns the acceleration gain.
+   * Returns the acceleration gain in V/(m/s²).
   *
   * @return The acceleration gain.
   */
@@ -108,67 +132,83 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
  }

  /**
-   * Calculates the feedforward from the gains and setpoints.
+   * Returns the period in seconds.
+   *
+   * @return The period in seconds.
+   */
+  public double getDt() {
+    return m_dt;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming continuous control.
   *
   * @param velocity The velocity setpoint.
   * @param acceleration The acceleration setpoint.
   * @return The computed feedforward.
   */
+  @SuppressWarnings("removal")
+  @Deprecated(forRemoval = true, since = "2025")
  public double calculate(double velocity, double acceleration) {
    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
  }

  /**
-   * Calculates the feedforward from the gains and setpoints.
+   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
+   * (acceleration is assumed to be zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  @SuppressWarnings("removal")
+  @Deprecated(forRemoval = true, since = "2025")
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete control when the
+   * setpoint does not change.
+   *
+   * @param currentVelocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public Voltage calculate(LinearVelocity currentVelocity) {
+    return calculate(currentVelocity, currentVelocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete control.
   *
   * <p>Note this method is inaccurate when the velocity crosses 0.
   *
   * @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) {
-    // Discretize the affine model.
-    //
-    //   dx/dt = Ax + Bu + c
-    //   dx/dt = Ax + B(u + B⁺c)
-    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
-    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
-    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
-    //
-    // Solve for uₖ.
-    //
-    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
-    //
-    // For an elevator with the model
-    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
-    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
-    // assuming sgn(x) is a constant for the duration of the step.
-    //
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
-    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
-    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
-
-    var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
-    var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
-
-    return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
-   * zero).
-   *
-   * @param velocity The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity) {
-    return calculate(velocity, 0);
+  public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) {
+    // See wpimath/algorithms.md#Elevator_feedforward for derivation
+    if (ka == 0.0) {
+      output.mut_replace(
+          ks * Math.signum(nextVelocity.in(MetersPerSecond))
+              + kg
+              + kv * nextVelocity.in(MetersPerSecond),
+          Volts);
+      return output;
+    } 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;
+      output.mut_replace(
+          kg
+              + ks * Math.signum(currentVelocity.magnitude())
+              + 1.0
+                  / B_d
+                  * (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)),
+          Volts);
+      return output;
+    }
  }

  // Rearranging the main equation from the calculate() method yields the
@@ -180,9 +220,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
   * simultaneously achievable - enter the acceleration constraint, and this will give you a
   * simultaneously-achievable velocity constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param acceleration The acceleration of the elevator.
-   * @return The maximum possible velocity at the given acceleration.
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
+   * @param acceleration The acceleration of the elevator, in (m/s²).
+   * @return The maximum possible velocity in (m/s) at the given acceleration.
   */
  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
    // Assume max velocity is positive
@@ -195,9 +235,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
   * simultaneously achievable - enter the acceleration constraint, and this will give you a
   * simultaneously-achievable velocity constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param acceleration The acceleration of the elevator.
-   * @return The minimum possible velocity at the given acceleration.
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
+   * @param acceleration The acceleration of the elevator, in (m/s²).
+   * @return The maximum possible velocity in (m/s) at the given acceleration.
   */
  public double minAchievableVelocity(double maxVoltage, double acceleration) {
    // Assume min velocity is negative, ks flips sign
@@ -210,9 +250,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
   * simultaneously achievable - enter the velocity constraint, and this will give you a
   * simultaneously-achievable acceleration constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param velocity The velocity of the elevator.
-   * @return The maximum possible acceleration at the given velocity.
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
+   * @param velocity The velocity of the elevator, in (m/s)
+   * @return The maximum possible acceleration in (m/s²) at the given velocity.
   */
  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
@@ -224,11 +264,17 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
   * simultaneously achievable - enter the velocity constraint, and this will give you a
   * simultaneously-achievable acceleration constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param velocity The velocity of the elevator.
-   * @return The minimum possible acceleration at the given velocity.
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
+   * @param velocity The velocity of the elevator, in (m/s)
+   * @return The maximum possible acceleration in (m/s²) at the given velocity.
   */
  public double minAchievableAcceleration(double maxVoltage, double velocity) {
    return maxAchievableAcceleration(-maxVoltage, velocity);
  }
+
+  /** ElevatorFeedforward struct for serialization. */
+  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
+
+  /** ElevatorFeedforward protobuf for serialization. */
+  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -12,31 +12,36 @@ import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.PerUnit;
 import edu.wpi.first.units.TimeUnit;
 import edu.wpi.first.units.Unit;
+import edu.wpi.first.units.measure.MutVoltage;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;

 /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
 public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable {
-  /** The static gain. */
+  /** The static gain, in volts. */
  private final double ks;

-  /** The velocity gain. */
+  /** The velocity gain, in V/(units/s). */
  private final double kv;

-  /** The acceleration gain. */
+  /** The acceleration gain, in V/(units/s²). */
  private final double ka;

-  /** The period. */
+  /** The period, in seconds. */
  private final double m_dt;

+  // ** The calculated output voltage measure */
+  private final MutVoltage output = Volts.mutable(0.0);
+
  /**
-   * Creates a new SimpleMotorFeedforward with the specified gains and period. Units of the gain
-   * values will dictate units of the computed feedforward.
+   * Creates a new SimpleMotorFeedforward with the specified gains and period.
   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param ks The static gain in volts.
+   * @param kv The velocity gain in V/(units/s).
+   * @param ka The acceleration gain in V/(units/s²).
   * @param dtSeconds The period in seconds.
   * @throws IllegalArgumentException for kv &lt; zero.
   * @throws IllegalArgumentException for ka &lt; zero.
@@ -61,11 +66,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria

  /**
   * Creates a new SimpleMotorFeedforward with the specified gains and period. The period is
-   * defaulted to 20 ms. Units of the gain values will dictate units of the computed feedforward.
+   * defaulted to 20 ms.
   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param ks The static gain in volts.
+   * @param kv The velocity gain in V/(units/s).
+   * @param ka The acceleration gain in V/(units/s²).
   * @throws IllegalArgumentException for kv &lt; zero.
   * @throws IllegalArgumentException for ka &lt; zero.
   */
@@ -75,45 +82,51 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria

  /**
   * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
-   * to zero. The period is defaulted to 20 ms. Units of the gain values will dictate units of the
-   * computed feedforward.
+   * to zero. The period is defaulted to 20 ms.
   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param ks The static gain in volts.
+   * @param kv The velocity gain in V/(units/s).
+   * @throws IllegalArgumentException for kv &lt; zero.
   */
  public SimpleMotorFeedforward(double ks, double kv) {
-    this(ks, kv, 0, 0.020);
+    this(ks, kv, 0);
  }

  /**
-   * Returns the static gain.
+   * Returns the static gain in volts.
   *
-   * @return The static gain.
+   * @return The static gain in volts.
   */
  public double getKs() {
    return ks;
  }

  /**
-   * Returns the velocity gain.
+   * Returns the velocity gain in V/(units/s).
   *
-   * @return The velocity gain.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @return The velocity gain in V/(units/s).
   */
  public double getKv() {
    return kv;
  }

  /**
-   * Returns the acceleration gain.
+   * Returns the acceleration gain in V/(units/s²).
   *
-   * @return The acceleration gain.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @return The acceleration gain in V/(units/s²).
   */
  public double getKa() {
    return ka;
  }

  /**
-   * Returns the period.
+   * Returns the period in seconds.
   *
   * @return The period in seconds.
   */
@@ -122,7 +135,7 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
  }

  /**
-   * Calculates the feedforward from the gains and setpoints.
+   * Calculates the feedforward from the gains and setpoints assuming continuous control.
   *
   * @param velocity The velocity setpoint.
   * @param acceleration The acceleration setpoint.
@@ -136,8 +149,8 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
  }

  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
-   * zero).
+   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
+   * (acceleration is assumed to be zero).
   *
   * @param velocity The velocity setpoint.
   * @return The computed feedforward.
@@ -164,6 +177,8 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
  /**
   * Calculates the feedforward from the gains and setpoints assuming discrete control.
   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
   * @param <U> The velocity parameter either as distance or angle.
   * @param currentVelocity The current velocity setpoint.
   * @param nextVelocity The next velocity setpoint.
@@ -172,67 +187,21 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
  public <U extends Unit> Voltage calculate(
      Measure<? extends PerUnit<U, TimeUnit>> currentVelocity,
      Measure<? extends PerUnit<U, TimeUnit>> nextVelocity) {
+    // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
    if (ka == 0.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 = 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());
+      output.mut_replace(
+          ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude(), Volts);
+      return output;
    } else {
-      //   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(
+      output.mut_replace(
          ks * Math.signum(currentVelocity.magnitude())
-              + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()));
+              + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()),
+          Volts);
+      return output;
    }
  }

@@ -242,9 +211,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
   * simultaneously achievable - enter the acceleration constraint, and this will give you a
   * simultaneously-achievable velocity constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param acceleration The acceleration of the motor.
-   * @return The maximum possible velocity at the given acceleration.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param acceleration The acceleration of the motor, in (units/s²).
+   * @return The maximum possible velocity in (units/s) at the given acceleration.
   */
  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
    // Assume max velocity is positive
@@ -257,9 +228,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
   * simultaneously achievable - enter the acceleration constraint, and this will give you a
   * simultaneously-achievable velocity constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param acceleration The acceleration of the motor.
-   * @return The minimum possible velocity at the given acceleration.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param acceleration The acceleration of the motor, in (units/s²).
+   * @return The maximum possible velocity in (units/s) at the given acceleration.
   */
  public double minAchievableVelocity(double maxVoltage, double acceleration) {
    // Assume min velocity is negative, ks flips sign
@@ -272,9 +245,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
   * simultaneously achievable - enter the velocity constraint, and this will give you a
   * simultaneously-achievable acceleration constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param velocity The velocity of the motor.
-   * @return The maximum possible acceleration at the given velocity.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param velocity The velocity of the motor, in (units/s).
+   * @return The maximum possible acceleration in (units/s²) at the given velocity.
   */
  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
@@ -286,9 +261,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
   * simultaneously achievable - enter the velocity constraint, and this will give you a
   * simultaneously-achievable acceleration constraint.
   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param velocity The velocity of the motor.
-   * @return The minimum possible acceleration at the given velocity.
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param velocity The velocity of the motor, in (units/s).
+   * @return The maximum possible acceleration in (units/s²) at the given velocity.
   */
  public double minAchievableAcceleration(double maxVoltage, double velocity) {
    return maxAchievableAcceleration(-maxVoltage, velocity);
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java
@@ -27,7 +27,7 @@ public class ArmFeedforwardProto implements Protobuf<ArmFeedforward, ProtobufArm

  @Override
  public ArmFeedforward unpack(ProtobufArmFeedforward msg) {
-    return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa());
+    return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt());
  }

  @Override
@@ -36,5 +36,6 @@ public class ArmFeedforwardProto implements Protobuf<ArmFeedforward, ProtobufArm
    msg.setKg(value.getKg());
    msg.setKv(value.getKv());
    msg.setKa(value.getKa());
+    msg.setDt(value.getDt());
  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java
@@ -28,7 +28,7 @@ public class ElevatorFeedforwardProto

  @Override
  public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) {
-    return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa());
+    return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt());
  }

  @Override
@@ -37,5 +37,6 @@ public class ElevatorFeedforwardProto
    msg.setKg(value.getKg());
    msg.setKv(value.getKv());
    msg.setKa(value.getKa());
+    msg.setDt(value.getDt());
  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java
@@ -21,12 +21,12 @@ public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {

  @Override
  public int getSize() {
-    return kSizeDouble * 4;
+    return kSizeDouble * 5;
  }

  @Override
  public String getSchema() {
-    return "double ks;double kg;double kv;double ka";
+    return "double ks;double kg;double kv;double ka;double dt";
  }

  @Override
@@ -35,7 +35,8 @@ public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
    double kg = bb.getDouble();
    double kv = bb.getDouble();
    double ka = bb.getDouble();
-    return new ArmFeedforward(ks, kg, kv, ka);
+    double dt = bb.getDouble();
+    return new ArmFeedforward(ks, kg, kv, ka, dt);
  }

  @Override
@@ -44,5 +45,6 @@ public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
    bb.putDouble(value.getKg());
    bb.putDouble(value.getKv());
    bb.putDouble(value.getKa());
+    bb.putDouble(value.getDt());
  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java
@@ -21,12 +21,12 @@ public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {

  @Override
  public int getSize() {
-    return kSizeDouble * 4;
+    return kSizeDouble * 5;
  }

  @Override
  public String getSchema() {
-    return "double ks;double kg;double kv;double ka";
+    return "double ks;double kg;double kv;double ka;double dt";
  }

  @Override
@@ -35,7 +35,8 @@ public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
    double kg = bb.getDouble();
    double kv = bb.getDouble();
    double ka = bb.getDouble();
-    return new ElevatorFeedforward(ks, kg, kv, ka);
+    double dt = bb.getDouble();
+    return new ElevatorFeedforward(ks, kg, kv, ka, dt);
  }

  @Override
@@ -44,5 +45,6 @@ public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
    bb.putDouble(value.getKg());
    bb.putDouble(value.getKv());
    bb.putDouble(value.getKa());
+    bb.putDouble(value.getDt());
  }
 }
--- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
@@ -19,6 +19,19 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
                                        units::unit_t<Velocity> currentVelocity,
                                        units::unit_t<Velocity> nextVelocity,
                                        units::second_t dt) const {
+  return Calculate(currentAngle, currentVelocity, nextVelocity);
+}
+
+units::volt_t ArmFeedforward::Calculate(
+    units::unit_t<Angle> currentAngle,
+    units::unit_t<Velocity> currentVelocity) const {
+  return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) +
+         kV * currentVelocity;
+}
+
+units::volt_t ArmFeedforward::Calculate(
+    units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
+    units::unit_t<Velocity> nextVelocity) const {
  using VarMat = sleipnir::VariableMatrix;

  // Arm dynamics
@@ -36,12 +49,12 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
  sleipnir::Variable u_k;

  // Initial guess
-  auto acceleration = (nextVelocity - currentVelocity) / dt;
+  auto acceleration = (nextVelocity - currentVelocity) / m_dt;
  u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
                kA * acceleration + kG * units::math::cos(currentAngle))
                   .value());

-  auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, dt);
+  auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, m_dt);

  // Minimize difference between desired and actual next velocity
  auto cost =
--- a/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
@@ -26,10 +26,11 @@ Java_edu_wpi_first_math_jni_ArmFeedforwardJNI_calculate
 {
  return frc::ArmFeedforward{units::volt_t{ks}, units::volt_t{kg},
                             units::unit_t<frc::ArmFeedforward::kv_unit>{kv},
-                             units::unit_t<frc::ArmFeedforward::ka_unit>{ka}}
+                             units::unit_t<frc::ArmFeedforward::ka_unit>{ka},
+                             units::second_t{dt}}
      .Calculate(units::radian_t{currentAngle},
                 units::radians_per_second_t{currentVelocity},
-                 units::radians_per_second_t{nextVelocity}, units::second_t{dt})
+                 units::radians_per_second_t{nextVelocity})
      .value();
 }

--- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -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

--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -37,11 +37,16 @@ class ElevatorFeedforward {
   * @param kG The gravity gain, in volts.
   * @param kV The velocity gain, in volt seconds per distance.
   * @param kA The acceleration gain, in volt seconds² per distance.
+   * @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 ElevatorFeedforward(
      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,55 +59,43 @@ class ElevatorFeedforward {
      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 velocity     The velocity setpoint, in distance per second.
-   * @param acceleration The acceleration setpoint, in distance 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.
   */
-  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
-                                    units::unit_t<Acceleration> acceleration =
-                                        units::unit_t<Acceleration>(0)) {
+  [[deprecated("Use the current/next velocity overload instead.")]]
+  units::volt_t Calculate(units::unit_t<Velocity> velocity,
+                          units::unit_t<Acceleration> acceleration) const {
    return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
  }

  /**
-   * Calculates the feedforward from the gains and setpoints.
+   * Calculates the feedforward from the gains and setpoints assuming continuous
+   * control.
   *
-   * @param currentVelocity The current velocity setpoint, in distance per
-   *                        second.
-   * @param nextVelocity    The next velocity setpoint, in distance 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<Velocity> currentVelocity,
                          units::unit_t<Velocity> nextVelocity,
                          units::second_t dt) const {
-    // Discretize the affine model.
-    //
-    //   dx/dt = Ax + Bu + c
-    //   dx/dt = Ax + B(u + B⁺c)
-    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
-    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
-    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
-    //
-    // Solve for uₖ.
-    //
-    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
-    //
-    // For an elevator with the model
-    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
-    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
-    // assuming sgn(x) is a constant for the duration of the step.
-    //
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
-    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    // See wpimath/algorithms.md#Elevator_feedforward for derivation
    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};

@@ -113,6 +106,46 @@ class ElevatorFeedforward {
           units::volt_t{feedforward.Calculate(r, nextR)(0)};
  }

+  /**
+   * Calculates the feedforward from the gains and setpoint assuming discrete
+   * control. Use this method when the setpoint does not change.
+   *
+   * @param currentVelocity The velocity setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(
+      units::unit_t<Velocity> currentVelocity) const {
+    return Calculate(currentVelocity, currentVelocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete
+   * control.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity    The next velocity setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(
+      units::unit_t<Velocity> currentVelocity,
+      units::unit_t<Velocity> nextVelocity) const {
+    // See wpimath/algorithms.md#Elevator_feedforward for derivation
+    if (kA == decltype(kA)(0)) {
+      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;
+      return kG + 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
  // formulas for the methods below:

@@ -222,6 +255,9 @@ class ElevatorFeedforward {

  /// The acceleration gain.
  units::unit_t<ka_unit> kA;
+
+  /** The period. */
+  units::second_t m_dt;
 };
 }  // namespace frc

--- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -7,6 +7,8 @@
 #include <gcem.hpp>
 #include <wpi/MathExtras.h>

+#include "units/angle.h"
+#include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
 #include "wpimath/MathShared.h"
@@ -18,6 +20,8 @@ namespace frc {
 * permanent-magnet DC motor.
 */
 template <class Distance>
+  requires std::same_as<units::meter, Distance> ||
+           std::same_as<units::radian, Distance>
 class SimpleMotorFeedforward {
 public:
  using Velocity =
@@ -35,6 +39,9 @@ class SimpleMotorFeedforward {
   * @param kV The velocity gain, in volt seconds per distance.
   * @param kA The acceleration gain, in volt seconds² per distance.
   * @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 SimpleMotorFeedforward(
      units::volt_t kS, units::unit_t<kv_unit> kV,
@@ -62,10 +69,11 @@ class SimpleMotorFeedforward {
  }

  /**
-   * Calculates the feedforward from the gains and setpoints.
+   * Calculates the feedforward from the gains and setpoints assuming continuous
+   * control.
   *
-   * @param velocity     The velocity setpoint, in distance per second.
-   * @param acceleration The acceleration setpoint, in distance 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.
   */
@@ -77,11 +85,10 @@ class SimpleMotorFeedforward {
  }

  /**
-   * Calculates the feedforward from the gains and setpoint.
-   * Use this method when the setpoint does not change.
+   * Calculates the feedforward from the gains and setpoint assuming discrete
+   * control. Use this method when the setpoint does not change.
   *
-   * @param setpoint The velocity setpoint, in distance per
-   *                        second.
+   * @param setpoint The velocity setpoint.
   * @return The computed feedforward, in volts.
   */
  constexpr units::volt_t Calculate(units::unit_t<Velocity> setpoint) const {
@@ -89,70 +96,22 @@ class SimpleMotorFeedforward {
  }

  /**
-   * Calculates the feedforward from the gains and setpoints.
+   * Calculates the feedforward from the gains and setpoints assuming discrete
+   * control.
   *
-   * @param currentVelocity The current velocity setpoint, in distance per
-   *                        second.
-   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity    The next velocity setpoint.
   * @return The computed feedforward, in volts.
   */
  constexpr units::volt_t Calculate(
      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)) {
-      // 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 {
-      //   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());
--- a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h
+++ b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h
@@ -10,7 +10,8 @@
 #include "units/length.h"

 // Everything is converted into units for
-// frc::SimpleMotorFeedforward<units::meters>
+// frc::SimpleMotorFeedforward<units::meters> or
+// frc::SimpleMotorFeedforward<units::radians>

 template <class Distance>
 struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
@@ -31,6 +32,6 @@ struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
 static_assert(
    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
 static_assert(
-    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::feet>>);
+    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::radians>>);

 #include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc"
--- a/wpimath/src/main/proto/controller.proto
+++ b/wpimath/src/main/proto/controller.proto
@@ -9,6 +9,7 @@ message ProtobufArmFeedforward {
  double kg = 2;
  double kv = 3;
  double ka = 4;
+  double dt = 5;
 }

 message ProtobufDifferentialDriveFeedforward {
@@ -23,6 +24,7 @@ message ProtobufElevatorFeedforward {
  double kg = 2;
  double kv = 3;
  double ka = 4;
+  double dt = 5;
 }

 message ProtobufSimpleMotorFeedforward {
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
@@ -4,6 +4,9 @@

 package edu.wpi.first.math.controller;

+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -68,19 +71,23 @@ class ArmFeedforwardTest {
   */
  private void calculateAndSimulate(
      double currentAngle, double currentVelocity, double nextVelocity, double dt) {
-    final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity, dt);
+    final double input =
+        m_armFF
+            .calculate(
+                Radians.of(currentAngle),
+                RadiansPerSecond.of(currentVelocity),
+                RadiansPerSecond.of(nextVelocity))
+            .in(Volts);
    assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
  }

  @Test
  void testCalculate() {
    // calculate(angle, angular velocity)
-    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
-    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
-
-    // calculate(angle, angular velocity, angular acceleration)
-    assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
-    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
+    assertEquals(
+        0.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(0)).in(Volts), 0.002);
+    assertEquals(
+        2.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(1)).in(Volts), 0.002);

    // calculate(currentAngle, currentVelocity, nextAngle, dt)
    calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
@@ -4,6 +4,8 @@

 package edu.wpi.first.math.controller;

+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -24,10 +26,8 @@ class ElevatorFeedforwardTest {

  @Test
  void testCalculate() {
-    assertEquals(1, m_elevatorFF.calculate(0), 0.002);
-    assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
-    assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
-    assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
+    assertEquals(1, m_elevatorFF.calculate(MetersPerSecond.of(0)).in(Volts), 0.002);
+    assertEquals(4.5, m_elevatorFF.calculate(MetersPerSecond.of(2)).in(Volts), 0.002);

    var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
    var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
@@ -38,7 +38,7 @@ class ElevatorFeedforwardTest {
    var nextR = VecBuilder.fill(3.0);
    assertEquals(
        plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
-        m_elevatorFF.calculate(2.0, 3.0, dt),
+        m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts),
        0.002);
  }

--- a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
@@ -62,7 +62,7 @@ void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
                          units::radians_per_second_t currentVelocity,
                          units::radians_per_second_t nextVelocity,
                          units::second_t dt) {
-  auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity, dt);
+  auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
  EXPECT_NEAR(nextVelocity.value(),
              Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
 }
@@ -80,17 +80,6 @@ TEST(ArmFeedforwardTest, Calculate) {
      armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s).value(), 2.5,
      0.002);

-  // Calculate(angle, angular velocity, angular acceleration)
-  EXPECT_NEAR(
-      armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 2_rad_per_s_sq)
-          .value(),
-      6.5, 0.002);
-  EXPECT_NEAR(
-      armFF
-          .Calculate(std::numbers::pi / 3 * 1_rad, -1_rad_per_s, 2_rad_per_s_sq)
-          .value(),
-      2.5, 0.002);
-
  // Calculate(currentAngle, currentVelocity, nextAngle, dt)
  CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
                       1.05_rad_per_s, 20_ms);
--- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
@@ -23,10 +23,6 @@ TEST(ElevatorFeedforwardTest, Calculate) {

  EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
-  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
-              0.002);
-  EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
-              0.002);

  frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
  frc::Matrixd<1, 1> B{1.0 / Ka.value()};
@@ -36,7 +32,7 @@ TEST(ElevatorFeedforwardTest, Calculate) {
  frc::Vectord<1> r{2.0};
  frc::Vectord<1> nextR{3.0};
  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
-              elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+              elevatorFF.Calculate(2_mps, 3_mps).value(), 0.002);
 }

 TEST(ElevatorFeedforwardTest, AchievableVelocity) {