[wpimath] Reorder TrapezoidProfile.calculate() arguments (#5874)

ProfiledPIDController and ExponentialProfile use current, then goal. This isn't a breaking change because this overload of calculate() is new for 2024.
2026-06-24 01:31:46 +00:00 · 2023-11-04 16:28:55 -07:00
parent 04a781b4d7
commit 201a42a3cd
15 changed files with 45 additions and 45 deletions
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -79,7 +79,7 @@ public class TrapezoidProfileCommand extends Command {
  @SuppressWarnings("removal")
  public void execute() {
    if (m_newAPI) {
-      m_output.accept(m_profile.calculate(m_timer.get(), m_goal.get(), m_currentState.get()));
+      m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
    } else {
      m_output.accept(m_profile.calculate(m_timer.get()));
    }
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -79,7 +79,7 @@ class TrapezoidProfileCommand
  void Initialize() override { m_timer.Restart(); }

  void Execute() override {
-    m_output(m_profile.Calculate(m_timer.Get(), m_goal(), m_currentState()));
+    m_output(m_profile.Calculate(m_timer.Get(), m_currentState(), m_goal()));
  }

  void End(bool interrupted) override { m_timer.Stop(); }
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -34,7 +34,7 @@ class Robot : public frc::TimedRobot {

    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
    // toward the goal while obeying the constraints.
-    m_setpoint = m_profile.Calculate(kDt, m_goal, m_setpoint);
+    m_setpoint = m_profile.Calculate(kDt, m_setpoint, m_goal);

    // Send setpoint to offboard controller PID
    m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
@@ -117,7 +117,7 @@ class Robot : public frc::TimedRobot {
      goal = {kLoweredPosition, 0_rad_per_s};
    }
    m_lastProfiledReference =
-        m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
+        m_profile.Calculate(20_ms, m_lastProfiledReference, goal);

    m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
                                    m_lastProfiledReference.velocity.value()});
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
@@ -117,7 +117,7 @@ class Robot : public frc::TimedRobot {
      goal = {kLoweredPosition, 0_fps};
    }
    m_lastProfiledReference =
-        m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
+        m_profile.Calculate(20_ms, m_lastProfiledReference, goal);

    m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
                                    m_lastProfiledReference.velocity.value()});
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -40,7 +40,7 @@ public class Robot extends TimedRobot {

    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
    // toward the goal while obeying the constraints.
-    m_setpoint = m_profile.calculate(kDt, m_goal, m_setpoint);
+    m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);

    // Send setpoint to offboard controller PID
    m_motor.setSetpoint(
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
@@ -126,7 +126,7 @@ 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 = m_profile.calculate(0.020, goal, m_lastProfiledReference);
+    m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
    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()));
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
@@ -130,7 +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 = m_profile.calculate(0.020, goal, m_lastProfiledReference);
+    m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
    m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);

    // Correct our Kalman filter's state vector estimate with encoder data.
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -347,7 +347,7 @@ public class ProfiledPIDController implements Sendable {
      m_setpoint.position = setpointMinDistance + measurement;
    }

-    m_setpoint = m_profile.calculate(getPeriod(), m_goal, m_setpoint);
+    m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal);
    return m_controller.calculate(measurement, m_setpoint.position);
  }

--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -29,7 +29,7 @@ import java.util.Objects;
 *
 * <pre><code>
 * previousProfiledReference =
- * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
 * </code></pre>
 *
 * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -212,11 +212,11 @@ public class TrapezoidProfile {
   * the profile was at time t = 0.
   *
   * @param t The time since the beginning of the profile.
-   * @param goal The desired state when the profile is complete.
   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
   * @return The position and velocity of the profile at time t.
   */
-  public State calculate(double t, State goal, State current) {
+  public State calculate(double t, State current, State goal) {
    m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
    m_current = direct(current);
    goal = direct(goal);
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -322,7 +322,7 @@ class ProfiledPIDController
      m_setpoint.position = setpointMinDistance + measurement;
    }

-    m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
+    m_setpoint = m_profile.Calculate(GetPeriod(), m_setpoint, m_goal);
    return m_controller.Calculate(measurement.value(),
                                  m_setpoint.position.value());
  }
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -29,8 +29,8 @@ namespace frc {
 * Run on update:
 * @code{.cpp}
 * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
- *                                               unprofiledReference,
- *                                               previousProfiledReference);
+ *                                               previousProfiledReference,
+ *                                               unprofiledReference);
 * @endcode
 *
 * where `unprofiledReference` is free to change between calls. Note that when
@@ -121,10 +121,10 @@ class TrapezoidProfile {
   * where the beginning of the profile was at time t = 0.
   *
   * @param t The time since the beginning of the profile.
-   * @param goal        The desired state when the profile is complete.
   * @param current     The initial state (usually the current state).
+   * @param goal        The desired state when the profile is complete.
   */
-  State Calculate(units::second_t t, State goal, State current);
+  State Calculate(units::second_t t, State current, State goal);

  /**
   * Returns the time left until a target distance in the profile is reached.
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -97,8 +97,8 @@ TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
 }
 template <class Distance>
 typename TrapezoidProfile<Distance>::State
-TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
-                                      State current) {
+TrapezoidProfile<Distance>::Calculate(units::second_t t, State current,
+                                      State goal) {
  m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
  m_current = Direct(current);
  goal = Direct(goal);
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -59,7 +59,7 @@ class TrapezoidProfileTest {

    TrapezoidProfile profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 450; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
    }
    assertEquals(state, goal);
  }
@@ -81,7 +81,7 @@ class TrapezoidProfileTest {
        profile = new TrapezoidProfile(constraints);
      }

-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
      double estimatedVel = (state.position - lastPos) / kDt;

      if (i >= 400) {
@@ -107,7 +107,7 @@ class TrapezoidProfileTest {

    TrapezoidProfile profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 400; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
    }
    assertEquals(state, goal);
  }
@@ -120,14 +120,14 @@ class TrapezoidProfileTest {

    TrapezoidProfile profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 200; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
    }
    assertNotEquals(state, goal);

    goal = new TrapezoidProfile.State(0.0, 0.0);
    profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 550; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
    }
    assertEquals(state, goal);
  }
@@ -141,13 +141,13 @@ class TrapezoidProfileTest {

    TrapezoidProfile profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 200; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
    }
    assertNear(constraints.maxVelocity, state.velocity, 10e-5);

    profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 2000; ++i) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
    }
    assertEquals(state, goal);
  }
@@ -160,7 +160,7 @@ class TrapezoidProfileTest {

    TrapezoidProfile profile = new TrapezoidProfile(constraints);
    for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
    }
  }
@@ -176,7 +176,7 @@ class TrapezoidProfileTest {
    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
    boolean reachedGoal = false;
    for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
      if (!reachedGoal && state.equals(goal)) {
        // Expected value using for loop index is just an approximation since
        // the time left in the profile doesn't increase linearly at the
@@ -198,7 +198,7 @@ class TrapezoidProfileTest {
    double predictedTimeLeft = profile.timeLeftUntil(1);
    boolean reachedGoal = false;
    for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
      if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
        reachedGoal = true;
@@ -217,7 +217,7 @@ class TrapezoidProfileTest {
    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
    boolean reachedGoal = false;
    for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
      if (!reachedGoal && state.equals(goal)) {
        // Expected value using for loop index is just an approximation since
        // the time left in the profile doesn't increase linearly at the
@@ -239,7 +239,7 @@ class TrapezoidProfileTest {
    double predictedTimeLeft = profile.timeLeftUntil(-1);
    boolean reachedGoal = false;
    for (int i = 0; i < 400; i++) {
-      state = profile.calculate(kDt, goal, state);
+      state = profile.calculate(kDt, state, goal);
      if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
        reachedGoal = true;
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -34,7 +34,7 @@ TEST(TrapezoidProfileTest, ReachesGoal) {

  frc::TrapezoidProfile<units::meter> profile{constraints};
  for (int i = 0; i < 450; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
  }
  EXPECT_EQ(state, goal);
 }
@@ -47,8 +47,8 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
  frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};

  frc::TrapezoidProfile<units::meter> profile{constraints};
-  auto state = profile.Calculate(kDt, goal,
-                                 frc::TrapezoidProfile<units::meter>::State{});
+  auto state = profile.Calculate(
+      kDt, frc::TrapezoidProfile<units::meter>::State{}, goal);

  auto lastPos = state.position;
  for (int i = 0; i < 1600; ++i) {
@@ -57,7 +57,7 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
      profile = frc::TrapezoidProfile<units::meter>{constraints};
    }

-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
    auto estimatedVel = (state.position - lastPos) / kDt;

    if (i >= 400) {
@@ -83,7 +83,7 @@ TEST(TrapezoidProfileTest, Backwards) {

  frc::TrapezoidProfile<units::meter> profile{constraints};
  for (int i = 0; i < 400; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
  }
  EXPECT_EQ(state, goal);
 }
@@ -96,14 +96,14 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {

  frc::TrapezoidProfile<units::meter> profile{constraints};
  for (int i = 0; i < 200; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
  }
  EXPECT_NE(state, goal);

  goal = {0.0_m, 0.0_mps};
  profile = frc::TrapezoidProfile<units::meter>{constraints};
  for (int i = 0; i < 550; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
  }
  EXPECT_EQ(state, goal);
 }
@@ -117,13 +117,13 @@ TEST(TrapezoidProfileTest, TopSpeed) {

  frc::TrapezoidProfile<units::meter> profile{constraints};
  for (int i = 0; i < 200; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
  }
  EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);

  profile = frc::TrapezoidProfile<units::meter>{constraints};
  for (int i = 0; i < 2000; ++i) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
  }
  EXPECT_EQ(state, goal);
 }
@@ -136,7 +136,7 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {

  frc::TrapezoidProfile<units::meter> profile{constraints};
  for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
  }
 }
@@ -155,7 +155,7 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
  bool reachedGoal = false;
  for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
    if (!reachedGoal && state == goal) {
      // Expected value using for loop index is just an approximation since the
      // time left in the profile doesn't increase linearly at the endpoints
@@ -179,7 +179,7 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
  auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
  bool reachedGoal = false;
  for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
    if (!reachedGoal &&
        (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -202,7 +202,7 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
  bool reachedGoal = false;
  for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
    if (!reachedGoal && state == goal) {
      // Expected value using for loop index is just an approximation since the
      // time left in the profile doesn't increase linearly at the endpoints
@@ -226,7 +226,7 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
  auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
  bool reachedGoal = false;
  for (int i = 0; i < 400; i++) {
-    state = profile.Calculate(kDt, goal, state);
+    state = profile.Calculate(kDt, state, goal);
    if (!reachedGoal &&
        (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);