[wpilib] Replace MecanumDriveMotorVoltages with a functional interface (

wpilibsuite#6760)

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

@@ -38,6 +38,7 @@
  *
  * <p>This class is provided by the NewCommands VendorDep
  */
+@SuppressWarnings("removal")
 public class MecanumControllerCommand extends Command {
   private final Timer m_timer = new Timer();
   private final boolean m_usePID;
@@ -53,7 +54,7 @@ public class MecanumControllerCommand extends Command {
   private final PIDController m_frontRightController;
   private final PIDController m_rearRightController;
   private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
-  private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
+  private final MecanumVoltagesConsumer m_outputDriveVoltages;
   private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
   private double m_prevFrontLeftSpeedSetpoint; // m/s
   private double m_prevRearLeftSpeedSetpoint; // m/s
@@ -84,8 +85,7 @@ public class MecanumControllerCommand extends Command {
    * @param frontRightController The front right wheel velocity PID.
    * @param rearRightController The rear right wheel velocity PID.
    * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
-   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
-   *     voltages.
+   * @param outputDriveVoltages A MecanumVoltagesConsumer that consumes voltages of mecanum motors.
    * @param requirements The subsystems to require.
    */
   @SuppressWarnings("this-escape")
@@ -104,7 +104,7 @@ public MecanumControllerCommand(
       PIDController frontRightController,
       PIDController rearRightController,
       Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
-      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      MecanumVoltagesConsumer outputDriveVoltages,
       Subsystem... requirements) {
     m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
     m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
@@ -145,6 +145,73 @@ public MecanumControllerCommand(
     addRequirements(requirements);
   }
 
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+   * 12 as a voltage output to the motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+   * 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 feedforward The feedforward to use for the drivetrain.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
+   *     step.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController The front left wheel velocity PID.
+   * @param rearLeftController The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController The rear right wheel velocity PID.
+   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
+   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
+   *     voltages.
+   * @param requirements The subsystems to require.
+   */
+  @Deprecated(since = "2025", forRemoval = true)
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SimpleMotorFeedforward feedforward,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      Supplier<Rotation2d> desiredRotation,
+      double maxWheelVelocityMetersPerSecond,
+      PIDController frontLeftController,
+      PIDController rearLeftController,
+      PIDController frontRightController,
+      PIDController rearRightController,
+      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        feedforward,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        desiredRotation,
+        maxWheelVelocityMetersPerSecond,
+        frontLeftController,
+        rearLeftController,
+        frontRightController,
+        rearRightController,
+        currentWheelSpeeds,
+        (frontLeft, frontRight, rearLeft, rearRight) ->
+            outputDriveVoltages.accept(
+                new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)),
+        requirements);
+  }
+
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow the provided
    * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
@@ -172,8 +239,7 @@ public MecanumControllerCommand(
    * @param frontRightController The front right wheel velocity PID.
    * @param rearRightController The rear right wheel velocity PID.
    * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
-   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
-   *     voltages.
+   * @param outputDriveVoltages A MecanumVoltagesConsumer that consumes voltages of mecanum motors.
    * @param requirements The subsystems to require.
    */
   public MecanumControllerCommand(
@@ -190,7 +256,7 @@ public MecanumControllerCommand(
       PIDController frontRightController,
       PIDController rearRightController,
       Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
-      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      MecanumVoltagesConsumer outputDriveVoltages,
       Subsystem... requirements) {
     this(
         trajectory,
@@ -212,6 +278,74 @@ public MecanumControllerCommand(
         requirements);
   }
 
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+   * 12 as a voltage output to the motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
+   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
+   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
+   * should be used.
+   *
+   * @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 feedforward The feedforward to use for the drivetrain.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController The front left wheel velocity PID.
+   * @param rearLeftController The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController The rear right wheel velocity PID.
+   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
+   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
+   *     voltages.
+   * @param requirements The subsystems to require.
+   */
+  @Deprecated(since = "2025", forRemoval = true)
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SimpleMotorFeedforward feedforward,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      double maxWheelVelocityMetersPerSecond,
+      PIDController frontLeftController,
+      PIDController rearLeftController,
+      PIDController frontRightController,
+      PIDController rearRightController,
+      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        feedforward,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        maxWheelVelocityMetersPerSecond,
+        frontLeftController,
+        rearLeftController,
+        frontRightController,
+        rearRightController,
+        currentWheelSpeeds,
+        (frontLeft, frontRight, rearLeft, rearRight) ->
+            outputDriveVoltages.accept(
+                new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)),
+        requirements);
+  }
+
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow the provided
    * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
@@ -403,8 +537,7 @@ public void execute() {
                   m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
 
       m_outputDriveVoltages.accept(
-          new MecanumDriveMotorVoltages(
-              frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
+          frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput);
 
     } else {
       m_outputWheelSpeeds.accept(
@@ -425,4 +558,22 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
+
+  /** A consumer to represent an operation on the voltages of a mecanum drive. */
+  @FunctionalInterface
+  public interface MecanumVoltagesConsumer {
+    /**
+     * Accepts the voltages to perform some operation with them.
+     *
+     * @param frontLeftVoltage The voltage of the front left motor.
+     * @param frontRightVoltage The voltage of the front right motor.
+     * @param rearLeftVoltage The voltage of the rear left motor.
+     * @param rearRightVoltage The voltage of the rear left motor.
+     */
+    void accept(
+        double frontLeftVoltage,
+        double frontRightVoltage,
+        double rearLeftVoltage,
+        double rearRightVoltage);
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java

@@ -5,7 +5,6 @@
 package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
 import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
@@ -125,11 +124,15 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ
   }
 
   /** Sets the front left drive MotorController to a voltage. */
-  public void setDriveMotorControllersVolts(MecanumDriveMotorVoltages volts) {
-    m_frontLeft.setVoltage(volts.frontLeftVoltage);
-    m_rearLeft.setVoltage(volts.rearLeftVoltage);
-    m_frontRight.setVoltage(volts.frontRightVoltage);
-    m_rearRight.setVoltage(volts.rearRightVoltage);
+  public void setDriveMotorControllersVolts(
+      double frontLeftVoltage,
+      double frontRightVoltage,
+      double rearLeftVoltage,
+      double rearRightVoltage) {
+    m_frontLeft.setVoltage(frontLeftVoltage);
+    m_rearLeft.setVoltage(rearLeftVoltage);
+    m_frontRight.setVoltage(frontRightVoltage);
+    m_rearRight.setVoltage(rearRightVoltage);
   }
 
   /** Resets the drive encoders to currently read a position of 0. */