[wpimath] Add SimpleMotorFeedforward::Calculate(velocity, nextVelocity) overload (#3183)

This is often more convenient than using the overload with velocity and
acceleration.

Fixes #3160.

wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h

@@ -6,6 +6,9 @@
 
 #include <wpi/MathExtras.h>
 
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "units/time.h"
 #include "units/voltage.h"
 
@@ -52,6 +55,31 @@ class SimpleMotorFeedforward {
     return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint, in distance per
+   *                        second.
+   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Eigen::Matrix<double, 1, 1> r;
+    r << currentVelocity.template to<double>();
+
+    Eigen::Matrix<double, 1, 1> nextR;
+    nextR << nextVelocity.template to<double>();
+
+    return kS * wpi::sgn(currentVelocity.template to<double>()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below: