[wpimath] Refactor TrapezoidProfile API (#5457)

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

@@ -17,13 +17,13 @@ public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
         // The motion profile to be executed
         new TrapezoidProfile(
             // The motion profile constraints
-            new TrapezoidProfile.Constraints(0, 0),
-            // Goal state
-            new TrapezoidProfile.State(),
-            // Initial state
-            new TrapezoidProfile.State()),
+            new TrapezoidProfile.Constraints(0, 0)),
         state -> {
           // Use current trajectory state here
-        });
+        },
+        // Goal state
+        () -> new TrapezoidProfile.State(),
+        // Current state
+        () -> new TrapezoidProfile.State());
   }
 }

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

@@ -72,11 +72,13 @@ public class RobotContainer {
                         // Limit the max acceleration and velocity
                         new TrapezoidProfile.Constraints(
                             DriveConstants.kMaxSpeedMetersPerSecond,
-                            DriveConstants.kMaxAccelerationMetersPerSecondSquared),
-                        // End at desired position in meters; implicitly starts at 0
-                        new TrapezoidProfile.State(3, 0)),
+                            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
+                    () -> new TrapezoidProfile.State(),
                     // Require the drive
                     m_robotDrive)
                 .beforeStarting(m_robotDrive::resetEncoders)

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

@@ -23,11 +23,13 @@ public class DriveDistanceProfiled extends TrapezoidProfileCommand {
             // Limit the max acceleration and velocity
             new TrapezoidProfile.Constraints(
                 DriveConstants.kMaxSpeedMetersPerSecond,
-                DriveConstants.kMaxAccelerationMetersPerSecondSquared),
-            // End at desired position in meters; implicitly starts at 0
-            new TrapezoidProfile.State(meters, 0)),
+                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
+        () -> new TrapezoidProfile.State(),
         // Require the drive
         drive);
     // Reset drive encoders since we're starting at 0

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java

@@ -33,17 +33,16 @@ public class Robot extends TimedRobot {
     if (m_joystick.getRawButtonPressed(2)) {
       m_goal = new TrapezoidProfile.State(5, 0);
     } else if (m_joystick.getRawButtonPressed(3)) {
-      m_goal = new TrapezoidProfile.State(0, 0);
+      m_goal = new TrapezoidProfile.State();
     }
 
     // Create a motion profile with the given maximum velocity and maximum
-    // acceleration constraints for the next setpoint, the desired goal, and the
-    // current setpoint.
-    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
+    // acceleration constraints for the next setpoint.
+    var profile = new TrapezoidProfile(m_constraints);
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.
-    m_setpoint = profile.calculate(kDt);
+    m_setpoint = profile.calculate(kDt, m_goal, m_setpoint);
 
     // Send setpoint to offboard controller PID
     m_motor.setSetpoint(

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java

@@ -41,10 +41,11 @@ public class Robot extends TimedRobot {
   // the motors, this number should be greater than one.
   private static final double kArmGearing = 10.0;
 
-  private final TrapezoidProfile.Constraints m_constraints =
-      new TrapezoidProfile.Constraints(
-          Units.degreesToRadians(45),
-          Units.degreesToRadians(90)); // Max arm speed and acceleration.
+  private final TrapezoidProfile m_profile =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(
+              Units.degreesToRadians(45),
+              Units.degreesToRadians(90))); // Max arm speed and acceleration.
   private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
 
   // The plant holds a state-space model of our arm. This system has the following properties:
@@ -125,10 +126,8 @@ public class Robot extends TimedRobot {
       goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
     }
     // Step our TrapezoidalProfile forward 20ms and set it as our next reference
-    m_lastProfiledReference =
-        (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
+    m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
     m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
-
     // Correct our Kalman filter's state vector estimate with encoder data.
     m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java

@@ -43,9 +43,11 @@ public class Robot extends TimedRobot {
   // the motors, this number should be greater than one.
   private static final double kElevatorGearing = 6.0;
 
-  private final TrapezoidProfile.Constraints m_constraints =
-      new TrapezoidProfile.Constraints(
-          Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
+  private final TrapezoidProfile m_profile =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(
+              Units.feetToMeters(3.0),
+              Units.feetToMeters(6.0))); // Max elevator speed and acceleration.
   private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
 
   /* The plant holds a state-space model of our elevator. This system has the following properties:
@@ -128,8 +130,7 @@ public class Robot extends TimedRobot {
       goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
     }
     // Step our TrapezoidalProfile forward 20ms and set it as our next reference
-    m_lastProfiledReference =
-        (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
+    m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
     m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
 
     // Correct our Kalman filter's state vector estimate with encoder data.