From 74ba9b11d5c853ae95d620d54a3037cc29926062 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 4 Nov 2023 16:28:55 -0700 Subject: [PATCH] [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. --- .../command/TrapezoidProfileCommand.java | 2 +- .../frc2/command/TrapezoidProfileCommand.h | 2 +- .../ElevatorTrapezoidProfile/cpp/Robot.cpp | 2 +- .../cpp/examples/StateSpaceArm/cpp/Robot.cpp | 2 +- .../examples/StateSpaceElevator/cpp/Robot.cpp | 2 +- .../elevatortrapezoidprofile/Robot.java | 2 +- .../wpilibj/examples/statespacearm/Robot.java | 2 +- .../examples/statespaceelevator/Robot.java | 2 +- .../controller/ProfiledPIDController.java | 2 +- .../math/trajectory/TrapezoidProfile.java | 6 ++-- .../frc/controller/ProfiledPIDController.h | 2 +- .../include/frc/trajectory/TrapezoidProfile.h | 8 +++--- .../frc/trajectory/TrapezoidProfile.inc | 4 +-- .../math/trajectory/TrapezoidProfileTest.java | 24 ++++++++-------- .../cpp/trajectory/TrapezoidProfileTest.cpp | 28 +++++++++---------- 15 files changed, 45 insertions(+), 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 index 57185a9df69..50416b6c30f 100644 --- 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 index 69cc8d8b238..289b1cb455e 100644 --- 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 index fb3a70dc8cd..63cd55d974f 100644 --- 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 index f6a9eeaf940..38e9262613f 100644 --- 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 index 3b837933309..ceadd8392bf 100644 --- 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/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 index 2acb5bcd39a..938132ba00d 100644 --- 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 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 index 69997d17f10..1b24a612501 100644 --- 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 index 20695212cbe..cb196c814cd 100644 --- 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 index 80311f5bfb3..ba29fa12c43 100644 --- 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 index fd7494ca25d..d29ed107c99 100644 --- 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 @@ * * 

  * previousProfiledReference =
- * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
  *
* *

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 index 8f211c62c4c..39dc3add2cd 100644 --- 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 index 73aab385069..18002462a03 100644 --- 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 index 24e0a46b80d..c970a791d57 100644 --- 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::Calculate(units::second_t t) const { } template typename TrapezoidProfile::State -TrapezoidProfile::Calculate(units::second_t t, State goal, - State current) { +TrapezoidProfile::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 index fb28b6957f1..64dfa4784d3 100644 --- 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 index b2e5b187e50..09c1ed54bd6 100644 --- 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 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::State goal{12_m, 0_mps}; frc::TrapezoidProfile profile{constraints}; - auto state = profile.Calculate(kDt, goal, - frc::TrapezoidProfile::State{}); + auto state = profile.Calculate( + kDt, frc::TrapezoidProfile::State{}, goal); auto lastPos = state.position; for (int i = 0; i < 1600; ++i) { @@ -57,7 +57,7 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) { profile = frc::TrapezoidProfile{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 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 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{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 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{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 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(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(predictedTimeLeft), i / 100.0, 2e-2);