[wpimath] Remove Units class from Elevator and Arm Feedforwards (#7570)

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java

@@ -4,9 +4,6 @@
 
 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;
@@ -114,9 +111,7 @@ public void reachGoal(double goal) {
     // 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(MetersPerSecond.of(m_setpoint.velocity), MetersPerSecond.of(next.velocity))
-            .in(Volts);
+        m_feedforward.calculateWithVelocities(m_setpoint.velocity, next.velocity);
 
     m_motor.setVoltage(pidOutput + feedforwardOutput);
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java

@@ -4,9 +4,6 @@
 
 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;
@@ -54,8 +51,6 @@ public void teleopPeriodic() {
     // Run controller and update motor output
     m_motor.setVoltage(
         m_controller.calculate(m_encoder.getDistance())
-            + m_feedforward
-                .calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity))
-                .in(Volts));
+            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java

@@ -4,9 +4,6 @@
 
 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;
@@ -104,8 +101,7 @@ public void reachGoal(double goal) {
 
     // With the setpoint value we run PID control like normal
     double pidOutput = m_controller.calculate(m_encoder.getDistance());
-    double feedforwardOutput =
-        m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts);
+    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
     m_motor.setVoltage(pidOutput + feedforwardOutput);
   }
 

wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java

@@ -4,16 +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 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.Voltage;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -167,8 +160,6 @@ public double calculate(
    * @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);
   }
@@ -192,45 +183,20 @@ public double 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) {
-    return Volts.of(
-        kg * Math.cos(currentAngle.in(Radians))
-            + ks * Math.signum(currentVelocity.in(RadiansPerSecond))
-            + kv * currentVelocity.in(RadiansPerSecond));
-  }
-
   /**
    * 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.
+   * @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.
    * @return The computed feedforward in volts.
    */
-  public Voltage calculate(
-      Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
-    return Volts.of(
-        ArmFeedforwardJNI.calculate(
-            ks,
-            kv,
-            ka,
-            kg,
-            currentAngle.in(Radians),
-            currentVelocity.in(RadiansPerSecond),
-            nextVelocity.in(RadiansPerSecond),
-            m_dt));
+  public double calculateWithVelocities(
+      double currentAngle, double currentVelocity, double nextVelocity) {
+    return ArmFeedforwardJNI.calculate(
+        ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt);
   }
 
   // Rearranging the main equation from the calculate() method yields the

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

@@ -4,13 +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 edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
 import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
-import edu.wpi.first.units.measure.LinearVelocity;
-import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -156,50 +151,31 @@ public double calculate(double velocity, double acceleration) {
    * @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.
-   * @return The computed feedforward.
+   * @param currentVelocity The current velocity setpoint in meters per second.
+   * @param nextVelocity The next velocity setpoint in meters per second.
+   * @return The computed feedforward in volts.
    */
-  public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) {
+  public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
     // See wpimath/algorithms.md#Elevator_feedforward for derivation
     if (ka == 0.0) {
-      return Volts.of(
-          ks * Math.signum(nextVelocity.in(MetersPerSecond))
-              + kg
-              + kv * nextVelocity.in(MetersPerSecond));
+      return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
     } else {
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);
       double B_d = 1.0 / A * (A_d - 1.0) * B;
-      return Volts.of(
-          kg
-              + ks * Math.signum(currentVelocity.magnitude())
-              + 1.0
-                  / B_d
-                  * (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)));
+      return kg
+          + ks * Math.signum(currentVelocity)
+          + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
     }
   }
 

wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java

@@ -4,9 +4,6 @@
 
 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;
@@ -72,22 +69,15 @@ private Matrix<N2, N1> simulate(
   private void calculateAndSimulate(
       double currentAngle, double currentVelocity, double nextVelocity, double dt) {
     final double input =
-        m_armFF
-            .calculate(
-                Radians.of(currentAngle),
-                RadiansPerSecond.of(currentVelocity),
-                RadiansPerSecond.of(nextVelocity))
-            .in(Volts);
+        m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
     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(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);
+    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
+    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
 
     // calculate(currentAngle, currentVelocity, nextAngle, dt)
     calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);

wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java

@@ -4,8 +4,6 @@
 
 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;
@@ -26,8 +24,8 @@ class ElevatorFeedforwardTest {
 
   @Test
   void testCalculate() {
-    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);
+    assertEquals(1, m_elevatorFF.calculate(0.0), 0.002);
+    assertEquals(4.5, m_elevatorFF.calculate(2.0), 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 +36,7 @@ void testCalculate() {
     var nextR = VecBuilder.fill(3.0);
     assertEquals(
         plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
-        m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts),
+        m_elevatorFF.calculateWithVelocities(2.0, 3.0),
         0.002);
   }