[wpimath] Reorder TrapezoidProfile.calculate() arguments (wpilibsuite…

…#5874) ProfiledPIDController and ExponentialProfile use current, then goal. This isn't a breaking change because this overload of calculate() is new for 2024.
Starlight220 · Dec 1, 2023 · 74ba9b1 · 74ba9b1
1 parent 04a781b
commit 74ba9b1
Show file tree

Hide file tree

Showing 15 changed files with 45 additions and 45 deletions.
diff --git 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 void initialize() {
   @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()));
     }

diff --git 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(); }

diff --git 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,

diff --git 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()});

diff --git 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()});

diff --git a/...Examples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/...Examples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -40,7 +40,7 @@ public void teleopPeriodic() {
 
     // 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(

diff --git 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 void teleopPeriodic() {
       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()));

diff --git 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 void teleopPeriodic() {
       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.

diff --git 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 double calculate(double measurement) {
       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);
   }
 

diff --git 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 @@
  *
  * <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 State calculate(double t) {
    * 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);

diff --git 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());
   }

diff --git 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.

diff --git 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);

diff --git 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 @@ void reachesGoal() {
 
     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 @@ void posContinuousUnderVelChange() {
         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 @@ void backwards() {
 
     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 @@ void switchGoalInMiddle() {
 
     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 @@ void topSpeed() {
 
     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 @@ void timingToCurrent() {
 
     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 @@ void timingToGoal() {
     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 @@ void timingBeforeGoal() {
     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 @@ void timingToNegativeGoal() {
     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 @@ void timingBeforeNegativeGoal() {
     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;

diff --git 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);