[wpimath] Add ArmFeedforward calculate() overload that takes current and next velocity instead of acceleration (#6540)

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

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -44,6 +44,33 @@ public final class WPIMathJNI {
     libraryLoaded = true;
   }
 
+  // ArmFeedforward wrappers
+
+  /**
+   * Obtain a feedforward voltage from a single jointed arm feedforward object.
+   *
+   * <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
+   *
+   * @param ks The ArmFeedforward's static gain in volts.
+   * @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
+   * @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
+   * @param kg The ArmFeedforward's gravity gain in volts.
+   * @param currentAngle The current angle in the calculation in radians.
+   * @param currentVelocity The current velocity in the calculation in radians per second.
+   * @param nextVelocity The next velocity in the calculation in radians per second.
+   * @param dt The time between velocity setpoints in seconds.
+   * @return The calculated feedforward in volts.
+   */
+  public static native double calculate(
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double nextVelocity,
+      double dt);
+
   // DARE wrappers
 
   /**

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

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
 import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -100,6 +101,22 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
     return calculate(positionRadians, velocity, 0);
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @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 dt Time between velocity setpoints in seconds.
+   * @return The computed feedforward in volts.
+   */
+  public double calculate(
+      double currentAngle, double currentVelocity, double nextVelocity, double dt) {
+    return WPIMathJNI.calculate(ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below: