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 57185a9df6..50416b6c30 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 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()));
}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 69cc8d8b23..289b1cb455 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 fb3a70dc8c..63cd55d974 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 f6a9eeaf94..38e9262613 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 3b83793330..ceadd8392b 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 2acb5bcd39..938132ba00 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 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(
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 69997d17f1..1b24a61250 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 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()));
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 20695212cb..cb196c814c 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 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.
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 80311f5bfb..ba29fa12c4 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 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);
}
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 fd7494ca25..d29ed107c9 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 @@ import java.util.Objects;
*
*
* 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 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);
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 8f211c62c4..39dc3add2c 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 73aab38506..18002462a0 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 24e0a46b80..c970a791d5 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 fb28b6957f..64dfa4784d 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 @@ 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;
diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index b2e5b187e5..09c1ed54bd 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);