diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java index e2dae9322a0..76b7d6e764a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java @@ -191,7 +191,7 @@ public State calculate(double t, State current, State goal) { var timing = calculateProfileTiming(current, inflectionPoint, goal, u); if (t < 0) { - return current; + return new State(current.position, current.velocity); } else if (t < timing.inflectionTime) { return new State( computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current)); @@ -200,7 +200,7 @@ public State calculate(double t, State current, State goal) { computeDistanceFromTime(t - timing.totalTime, -u, goal), computeVelocityFromTime(t - timing.totalTime, -u, goal)); } else { - return goal; + return new State(goal.position, goal.velocity); } }