Add feedforward components (#2045)

Add helper classes for computing feedforwards with parameters supplied by the characterization tool.

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java

@@ -14,6 +14,7 @@ import java.util.function.Supplier;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
@@ -45,9 +46,7 @@ public class RamseteCommand extends CommandBase {
   private final Trajectory m_trajectory;
   private final Supplier<Pose2d> m_pose;
   private final RamseteController m_follower;
-  private final double m_ks;
-  private final double m_kv;
-  private final double m_ka;
+  private final SimpleMotorFeedforward m_feedforward;
   private final DifferentialDriveKinematics m_kinematics;
   private final DoubleSupplier m_leftSpeed;
   private final DoubleSupplier m_rightSpeed;
@@ -67,12 +66,8 @@ public class RamseteCommand extends CommandBase {
    * @param trajectory                     The trajectory to follow.
    * @param pose                           A function that supplies the robot pose - use one of
    *                                       the odometry classes to provide this.
-   * @param controller                       The RAMSETE controller used to follow the trajectory.
-   * @param ksVolts                        Constant feedforward term for the robot drive.
-   * @param kvVoltSecondsPerMeter          Velocity-proportional feedforward term for the robot
-   *                                       drive.
-   * @param kaVoltSecondsSquaredPerMeter   Acceleration-proportional feedforward term for the robot
-   *                                       drive.
+   * @param controller                     The RAMSETE controller used to follow the trajectory.
+   * @param feedforward                    The feedforward to use for the drive.
    * @param kinematics                     The kinematics for the robot drivetrain.
    * @param leftWheelSpeedMetersPerSecond  A function that supplies the speed of the left side of
    *                                       the robot drive.
@@ -88,9 +83,7 @@ public class RamseteCommand extends CommandBase {
   public RamseteCommand(Trajectory trajectory,
                         Supplier<Pose2d> pose,
                         RamseteController controller,
-                        double ksVolts,
-                        double kvVoltSecondsPerMeter,
-                        double kaVoltSecondsSquaredPerMeter,
+                        SimpleMotorFeedforward feedforward,
                         DifferentialDriveKinematics kinematics,
                         DoubleSupplier leftWheelSpeedMetersPerSecond,
                         DoubleSupplier rightWheelSpeedMetersPerSecond,
@@ -101,9 +94,7 @@ public class RamseteCommand extends CommandBase {
     m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
     m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
     m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
-    m_ks = ksVolts;
-    m_kv = kvVoltSecondsPerMeter;
-    m_ka = kaVoltSecondsSquaredPerMeter;
+    m_feedforward = feedforward;
     m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
     m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
                                       "leftWheelSpeedMetersPerSecond",
@@ -146,9 +137,7 @@ public class RamseteCommand extends CommandBase {
     m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
     m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
 
-    m_ks = 0;
-    m_kv = 0;
-    m_ka = 0;
+    m_feedforward = null;
     m_leftSpeed = null;
     m_rightSpeed = null;
     m_leftController = null;
@@ -192,14 +181,12 @@ public class RamseteCommand extends CommandBase {
 
     if (m_usePID) {
       double leftFeedforward =
-          m_ks * Math.signum(leftSpeedSetpoint)
-              + m_kv * leftSpeedSetpoint
-              + m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt;
+          m_feedforward.calculate(leftSpeedSetpoint,
+                                  (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
 
       double rightFeedforward =
-          m_ks * Math.signum(rightSpeedSetpoint)
-              + m_kv * rightSpeedSetpoint
-              + m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt;
+          m_feedforward.calculate(rightSpeedSetpoint,
+                                  (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
 
       leftOutput = leftFeedforward
           + m_leftController.calculate(m_leftSpeed.getAsDouble(),

wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp

@@ -10,16 +10,10 @@
 using namespace frc2;
 using namespace units;
 
-template <typename T>
-int sgn(T val) {
-  return (T(0) < val) - (val < T(0));
-}
-
 RamseteCommand::RamseteCommand(
     frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-    frc::RamseteController controller, volt_t ks,
-    units::unit_t<voltsecondspermeter> kv,
-    units::unit_t<voltsecondssquaredpermeter> ka,
+    frc::RamseteController controller,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
     frc::DifferentialDriveKinematics kinematics,
     std::function<units::meters_per_second_t()> leftSpeed,
     std::function<units::meters_per_second_t()> rightSpeed,
@@ -29,9 +23,7 @@ RamseteCommand::RamseteCommand(
     : m_trajectory(trajectory),
       m_pose(pose),
       m_controller(controller),
-      m_ks(ks),
-      m_kv(kv),
-      m_ka(ka),
+      m_feedforward(feedforward),
       m_kinematics(kinematics),
       m_leftSpeed(leftSpeed),
       m_rightSpeed(rightSpeed),
@@ -52,9 +44,6 @@ RamseteCommand::RamseteCommand(
     : m_trajectory(trajectory),
       m_pose(pose),
       m_controller(controller),
-      m_ks(0),
-      m_kv(0),
-      m_ka(0),
       m_kinematics(kinematics),
       m_outputVel(output),
       m_usePID(false) {
@@ -84,12 +73,12 @@ void RamseteCommand::Execute() {
 
   if (m_usePID) {
     auto leftFeedforward =
-        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
-        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+        m_feedforward.Calculate(targetWheelSpeeds.left,
+            (targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
 
     auto rightFeedforward =
-        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
-        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+        m_feedforward.Calculate(targetWheelSpeeds.right,
+            (targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
 
     auto leftOutput =
         volt_t(m_leftController->Calculate(

wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h

@@ -13,6 +13,7 @@
 
 #include <frc/controller/PIDController.h>
 #include <frc/controller/RamseteController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/Trajectory.h>
@@ -41,12 +42,6 @@ namespace frc2 {
  * @see Trajectory
  */
 class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
-  using voltsecondspermeter =
-      units::compound_unit<units::volt, units::second,
-                           units::inverse<units::meter>>;
-  using voltsecondssquaredpermeter =
-      units::compound_unit<units::volt, units::squared<units::second>,
-                           units::inverse<units::meter>>;
 
  public:
   /**
@@ -63,11 +58,7 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
    * the odometry classes to provide this.
    * @param controller      The RAMSETE controller used to follow the
    * trajectory.
-   * @param ks              Constant feedforward term for the robot drive.
-   * @param kv              Velocity-proportional feedforward term for the robot
-   * drive.
-   * @param ka              Acceleration-proportional feedforward term for the
-   * robot drive.
+   * @param feedforward     A component for calculating the feedforward for the drive.
    * @param kinematics      The kinematics for the robot drivetrain.
    * @param leftSpeed       A function that supplies the speed of the left side
    * of the robot drive.
@@ -82,9 +73,8 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
    * @param requirements    The subsystems to require.
    */
   RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-                 frc::RamseteController controller, units::volt_t ks,
-                 units::unit_t<voltsecondspermeter> kv,
-                 units::unit_t<voltsecondssquaredpermeter> ka,
+                 frc::RamseteController controller,
+                 frc::SimpleMotorFeedforward<units::meters> feedforward,
                  frc::DifferentialDriveKinematics kinematics,
                  std::function<units::meters_per_second_t()> leftSpeed,
                  std::function<units::meters_per_second_t()> rightSpeed,
@@ -129,9 +119,7 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
   frc::Trajectory m_trajectory;
   std::function<frc::Pose2d()> m_pose;
   frc::RamseteController m_controller;
-  const units::volt_t m_ks;
-  const units::unit_t<voltsecondspermeter> m_kv;
-  const units::unit_t<voltsecondssquaredpermeter> m_ka;
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward;
   frc::DifferentialDriveKinematics m_kinematics;
   std::function<units::meters_per_second_t()> m_leftSpeed;
   std::function<units::meters_per_second_t()> m_rightSpeed;