Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)

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

@@ -8,7 +8,6 @@
 package edu.wpi.first.wpilibj2.command;
 
 import java.util.function.BiConsumer;
-import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
 import edu.wpi.first.wpilibj.Timer;
@@ -39,20 +38,18 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 @SuppressWarnings("PMD.TooManyFields")
 public class RamseteCommand extends CommandBase {
   private final Timer m_timer = new Timer();
-  private DifferentialDriveWheelSpeeds m_prevSpeeds;
-  private double m_prevTime;
   private final boolean m_usePID;
-
   private final Trajectory m_trajectory;
   private final Supplier<Pose2d> m_pose;
   private final RamseteController m_follower;
   private final SimpleMotorFeedforward m_feedforward;
   private final DifferentialDriveKinematics m_kinematics;
-  private final DoubleSupplier m_leftSpeed;
-  private final DoubleSupplier m_rightSpeed;
+  private final Supplier<DifferentialDriveWheelSpeeds> m_speeds;
   private final PIDController m_leftController;
   private final PIDController m_rightController;
   private final BiConsumer<Double, Double> m_output;
+  private DifferentialDriveWheelSpeeds m_prevSpeeds;
+  private double m_prevTime;
 
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
@@ -63,21 +60,19 @@ public class RamseteCommand extends CommandBase {
    * this
    * is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * @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 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.
-   * @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of
-   *                                       the robot drive.
-   * @param leftController                 The PIDController for the left side of the robot drive.
-   * @param rightController                The PIDController for the right side of the robot drive.
-   * @param outputVolts                    A function that consumes the computed left and right
-   *                                       outputs (in volts) for the robot drive.
-   * @param requirements                   The subsystems to require.
+   * @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 feedforward     The feedforward to use for the drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param wheelSpeeds     A function that supplies the speeds of the left and
+   *                        right sides of the robot drive.
+   * @param leftController  The PIDController for the left side of the robot drive.
+   * @param rightController The PIDController for the right side of the robot drive.
+   * @param outputVolts     A function that consumes the computed left and right
+   *                        outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
    */
   @SuppressWarnings("PMD.ExcessiveParameterList")
   public RamseteCommand(Trajectory trajectory,
@@ -85,8 +80,7 @@ public class RamseteCommand extends CommandBase {
                         RamseteController controller,
                         SimpleMotorFeedforward feedforward,
                         DifferentialDriveKinematics kinematics,
-                        DoubleSupplier leftWheelSpeedMetersPerSecond,
-                        DoubleSupplier rightWheelSpeedMetersPerSecond,
+                        Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
                         PIDController leftController,
                         PIDController rightController,
                         BiConsumer<Double, Double> outputVolts,
@@ -96,12 +90,7 @@ public class RamseteCommand extends CommandBase {
     m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
     m_feedforward = feedforward;
     m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
-    m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
-                                      "leftWheelSpeedMetersPerSecond",
-                                      "RamseteCommand");
-    m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond,
-                                       "rightWheelSpeedMetersPerSecond",
-                                       "RamseteCommand");
+    m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "RamseteCommand");
     m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
     m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
     m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
@@ -138,8 +127,7 @@ public class RamseteCommand extends CommandBase {
     m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
 
     m_feedforward = null;
-    m_leftSpeed = null;
-    m_rightSpeed = null;
+    m_speeds = null;
     m_leftController = null;
     m_rightController = null;
 
@@ -154,9 +142,9 @@ public class RamseteCommand extends CommandBase {
     var initialState = m_trajectory.sample(0);
     m_prevSpeeds = m_kinematics.toWheelSpeeds(
         new ChassisSpeeds(initialState.velocityMetersPerSecond,
-                          0,
-                          initialState.curvatureRadPerMeter
-                              * initialState.velocityMetersPerSecond));
+            0,
+            initialState.curvatureRadPerMeter
+                * initialState.velocityMetersPerSecond));
     m_timer.reset();
     m_timer.start();
     if (m_usePID) {
@@ -182,19 +170,19 @@ public class RamseteCommand extends CommandBase {
     if (m_usePID) {
       double leftFeedforward =
           m_feedforward.calculate(leftSpeedSetpoint,
-                                  (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
+              (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
 
       double rightFeedforward =
           m_feedforward.calculate(rightSpeedSetpoint,
-                                  (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
+              (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
 
       leftOutput = leftFeedforward
-          + m_leftController.calculate(m_leftSpeed.getAsDouble(),
-                                       leftSpeedSetpoint);
+          + m_leftController.calculate(m_speeds.get().leftMetersPerSecond,
+          leftSpeedSetpoint);
 
       rightOutput = rightFeedforward
-          + m_rightController.calculate(m_rightSpeed.getAsDouble(),
-                                        rightSpeedSetpoint);
+          + m_rightController.calculate(m_speeds.get().rightMetersPerSecond,
+          rightSpeedSetpoint);
     } else {
       leftOutput = leftSpeedSetpoint;
       rightOutput = rightSpeedSetpoint;