[commands] Fix and deprecate TrapezoidProfileCommand (#6722)

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json

@@ -119,16 +119,6 @@
     "replacename": "ReplaceMeSubsystem",
     "commandversion": 2
   },
-  {
-    "name": "TrapezoidProfileCommand",
-    "description": "A command that executes a trapezoidal motion profile.",
-    "tags": [
-      "trapezoidprofilecommand"
-    ],
-    "foldername": "trapezoidprofilecommand",
-    "replacename": "ReplaceMeTrapezoidProfileCommand",
-    "commandversion": 2
-  },
   {
     "name": "TrapezoidProfileSubsystem",
     "description": "A subsystem that executes a trapezoidal motion profile.",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java

@@ -1,29 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.wpilibj.commands.trapezoidprofilecommand;
-
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
-
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
-  /** Creates a new ReplaceMeTrapezoidProfileCommand. */
-  public ReplaceMeTrapezoidProfileCommand() {
-    super(
-        // The motion profile to be executed
-        new TrapezoidProfile(
-            // The motion profile constraints
-            new TrapezoidProfile.Constraints(0, 0)),
-        state -> {
-          // Use current trajectory state here
-        },
-        // Goal state
-        TrapezoidProfile.State::new,
-        // Current state
-        TrapezoidProfile.State::new);
-  }
-}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java

@@ -14,6 +14,7 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
  */
 public final class Constants {
   public static final class DriveConstants {
+    public static final double kDt = 0.02;
     public static final int kLeftMotor1Port = 0;
     public static final int kLeftMotor2Port = 1;
     public static final int kRightMotor1Port = 2;

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java

@@ -4,15 +4,11 @@
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 /**
@@ -61,28 +57,10 @@ public class RobotContainer {
     m_driverController.rightBumper().onTrue(m_driveHalfSpeed).onFalse(m_driveFullSpeed);
 
     // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
-    m_driverController.a().onTrue(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10));
+    m_driverController.a().onTrue(m_robotDrive.profiledDriveDistance(3).withTimeout(10));
 
-    // Do the same thing as above when the 'B' button is pressed, but defined inline
-    m_driverController
-        .b()
-        .onTrue(
-            new TrapezoidProfileCommand(
-                    new TrapezoidProfile(
-                        // Limit the max acceleration and velocity
-                        new TrapezoidProfile.Constraints(
-                            DriveConstants.kMaxSpeedMetersPerSecond,
-                            DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
-                    // Pipe the profile state to the drive
-                    setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
-                    // End at desired position in meters; implicitly starts at 0
-                    () -> new TrapezoidProfile.State(3, 0),
-                    // Current position
-                    TrapezoidProfile.State::new,
-                    // Require the drive
-                    m_robotDrive)
-                .beforeStarting(m_robotDrive::resetEncoders)
-                .withTimeout(10));
+    // Do the same thing as above when the 'B' button is pressed, but without resetting the encoders
+    m_driverController.b().onTrue(m_robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10));
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java

@@ -1,38 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;
-
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
-
-/** Drives a set distance using a motion profile. */
-public class DriveDistanceProfiled extends TrapezoidProfileCommand {
-  /**
-   * Creates a new DriveDistanceProfiled command.
-   *
-   * @param meters The distance to drive.
-   * @param drive The drive subsystem to use.
-   */
-  public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
-    super(
-        new TrapezoidProfile(
-            // Limit the max acceleration and velocity
-            new TrapezoidProfile.Constraints(
-                DriveConstants.kMaxSpeedMetersPerSecond,
-                DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
-        // Pipe the profile state to the drive
-        setpointState -> drive.setDriveStates(setpointState, setpointState),
-        // End at desired position in meters; implicitly starts at 0
-        () -> new TrapezoidProfile.State(meters, 0),
-        // Current position
-        TrapezoidProfile.State::new,
-        // Require the drive
-        drive);
-    // Reset drive encoders since we're starting at 0
-    drive.resetEncoders();
-  }
-}

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

@@ -9,10 +9,14 @@ import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -41,6 +45,16 @@ public class DriveSubsystem extends SubsystemBase {
   private final DifferentialDrive m_drive =
       new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 
+  // The trapezoid profile
+  private final TrapezoidProfile m_profile =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(
+              DriveConstants.kMaxSpeedMetersPerSecond,
+              DriveConstants.kMaxAccelerationMetersPerSecondSquared));
+
+  // The timer
+  private final Timer m_timer = new Timer();
+
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
     SendableRegistry.addChild(m_drive, m_leftLeader);
@@ -71,18 +85,33 @@ public class DriveSubsystem extends SubsystemBase {
   /**
    * Attempts to follow the given drive states using offboard PID.
    *
-   * @param left The left wheel state.
-   * @param right The right wheel state.
+   * @param currentLeft The current left wheel state.
+   * @param currentRight The current right wheel state.
+   * @param nextLeft The next left wheel state.
+   * @param nextRight The next right wheel state.
    */
-  public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
+  public void setDriveStates(
+      TrapezoidProfile.State currentLeft,
+      TrapezoidProfile.State currentRight,
+      TrapezoidProfile.State nextLeft,
+      TrapezoidProfile.State nextRight) {
+    // Feedforward is divided by battery voltage to normalize it to [-1, 1]
     m_leftLeader.setSetpoint(
         ExampleSmartMotorController.PIDMode.kPosition,
-        left.position,
-        m_feedforward.calculate(MetersPerSecond.of(left.velocity)).in(Volts));
+        currentLeft.position,
+        m_feedforward
+                .calculate(
+                    MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity))
+                .in(Volts)
+            / RobotController.getBatteryVoltage());
     m_rightLeader.setSetpoint(
         ExampleSmartMotorController.PIDMode.kPosition,
-        right.position,
-        m_feedforward.calculate(MetersPerSecond.of(right.velocity)).in(Volts));
+        currentRight.position,
+        m_feedforward
+                .calculate(
+                    MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity))
+                .in(Volts)
+            / RobotController.getBatteryVoltage());
   }
 
   /**
@@ -117,4 +146,80 @@ public class DriveSubsystem extends SubsystemBase {
   public void setMaxOutput(double maxOutput) {
     m_drive.setMaxOutput(maxOutput);
   }
+
+  /**
+   * Creates a command to drive forward a specified distance using a motion profile.
+   *
+   * @param distance The distance to drive forward.
+   * @return A command.
+   */
+  public Command profiledDriveDistance(double distance) {
+    return startRun(
+            () -> {
+              // Restart timer so profile setpoints start at the beginning
+              m_timer.restart();
+              resetEncoders();
+            },
+            () -> {
+              // Current state never changes, so we need to use a timer to get the setpoints we need
+              // to be at
+              var currentTime = m_timer.get();
+              var currentSetpoint =
+                  m_profile.calculate(currentTime, new State(), new State(distance, 0));
+              var nextSetpoint =
+                  m_profile.calculate(
+                      currentTime + DriveConstants.kDt, new State(), new State(distance, 0));
+              setDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, nextSetpoint);
+            })
+        .until(() -> m_profile.isFinished(0));
+  }
+
+  private double m_initialLeftDistance;
+  private double m_initialRightDistance;
+
+  /**
+   * Creates a command to drive forward a specified distance using a motion profile without
+   * resetting the encoders.
+   *
+   * @param distance The distance to drive forward.
+   * @return A command.
+   */
+  public Command dynamicProfiledDriveDistance(double distance) {
+    return startRun(
+            () -> {
+              // Restart timer so profile setpoints start at the beginning
+              m_timer.restart();
+              // Store distance so we know the target distance for each encoder
+              m_initialLeftDistance = getLeftEncoderDistance();
+              m_initialRightDistance = getRightEncoderDistance();
+            },
+            () -> {
+              // Current state never changes for the duration of the command, so we need to use a
+              // timer to get the setpoints we need to be at
+              var currentTime = m_timer.get();
+              var currentLeftSetpoint =
+                  m_profile.calculate(
+                      currentTime,
+                      new State(m_initialLeftDistance, 0),
+                      new State(m_initialLeftDistance + distance, 0));
+              var currentRightSetpoint =
+                  m_profile.calculate(
+                      currentTime,
+                      new State(m_initialRightDistance, 0),
+                      new State(m_initialRightDistance + distance, 0));
+              var nextLeftSetpoint =
+                  m_profile.calculate(
+                      currentTime + DriveConstants.kDt,
+                      new State(m_initialLeftDistance, 0),
+                      new State(m_initialLeftDistance + distance, 0));
+              var nextRightSetpoint =
+                  m_profile.calculate(
+                      currentTime + DriveConstants.kDt,
+                      new State(m_initialRightDistance, 0),
+                      new State(m_initialRightDistance + distance, 0));
+              setDriveStates(
+                  currentLeftSetpoint, currentRightSetpoint, nextLeftSetpoint, nextRightSetpoint);
+            })
+        .until(() -> m_profile.isFinished(0));
+  }
 }