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

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 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);
   }
@@ -192,45 +183,20 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
         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 class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
    * @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);
     }
   }