calculateWithVelocities

narmstro2020 · Dec 21, 2024 · e0f19ad · e0f19ad
1 parent 807dffe
commit e0f19ad
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 59 deletions.
diff --git 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,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,23 +183,6 @@ 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.
    *
@@ -219,18 +193,17 @@ public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) {
    * @param nextVelocity The next velocity setpoint.
    * @return The computed feedforward in volts.
    */
-  public Voltage calculate(
-      Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
-    return Volts.of(
-        ArmFeedforwardJNI.calculate(
+  public double calculateWithVelocities(
+      double currentAngle, double currentVelocity, double nextVelocity) {
+    return ArmFeedforwardJNI.calculate(
             ks,
             kv,
             ka,
             kg,
-            currentAngle.in(Radians),
-            currentVelocity.in(RadiansPerSecond),
-            nextVelocity.in(RadiansPerSecond),
-            m_dt));
+            currentAngle,
+            currentVelocity,
+            nextVelocity,
+            m_dt);
   }
 
   // Rearranging the main equation from the calculate() method yields the

diff --git 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,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,23 +151,10 @@ 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.
    *
@@ -182,24 +164,24 @@ public Voltage calculate(LinearVelocity currentVelocity) {
    * @param nextVelocity The next velocity setpoint.
    * @return The computed feedforward.
    */
-  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))
+      return 
+          ks * Math.signum(nextVelocity)
               + kg
-              + kv * nextVelocity.in(MetersPerSecond));
+              + 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(
+      return 
           kg
-              + ks * Math.signum(currentVelocity.magnitude())
+              + ks * Math.signum(currentVelocity)
               + 1.0
                   / B_d
-                  * (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)));
+                  * (nextVelocity - A_d * currentVelocity);
     }
   }