Add feedforward components (#2045)

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

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;