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 657ea24fe3..7d785a2d40 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 @@ -47,8 +47,6 @@ public class TrapezoidProfile { private final Constraints m_constraints; private State m_current; - private State m_goal; // TODO: Remove - private final boolean m_newAPI; // TODO: Remove private double m_endAccel; private double m_endFullSpeed; @@ -143,107 +141,6 @@ public class TrapezoidProfile { */ public TrapezoidProfile(Constraints constraints) { m_constraints = constraints; - m_newAPI = true; - } - - /** - * Constructs a TrapezoidProfile. - * - * @param constraints The constraints on the profile, like maximum velocity. - * @param goal The desired state when the profile is complete. - * @param initial The initial state (usually the current state). - * @deprecated Pass the desired and current state into calculate instead of constructing a new - * TrapezoidProfile with the desired and current state - */ - @Deprecated(since = "2024", forRemoval = true) - public TrapezoidProfile(Constraints constraints, State goal, State initial) { - m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1; - m_constraints = constraints; - m_current = direct(initial); - m_goal = direct(goal); - m_newAPI = false; - if (m_current.velocity > m_constraints.maxVelocity) { - m_current.velocity = m_constraints.maxVelocity; - } - - // Deal with a possibly truncated motion profile (with nonzero initial or - // final velocity) by calculating the parameters as if the profile began and - // ended at zero velocity - double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; - double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; - - double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration; - double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; - - // Now we can calculate the parameters as if it was a full trapezoid instead - // of a truncated one - - double fullTrapezoidDist = - cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd; - double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; - - double fullSpeedDist = - fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; - - // Handle the case where the profile never reaches full speed - if (fullSpeedDist < 0) { - accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); - fullSpeedDist = 0; - } - - m_endAccel = accelerationTime - cutoffBegin; - m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; - m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; - } - - /** - * Constructs a TrapezoidProfile. - * - * @param constraints The constraints on the profile, like maximum velocity. - * @param goal The desired state when the profile is complete. - * @deprecated Pass the desired and current state into calculate instead of constructing a new - * TrapezoidProfile with the desired and current state - */ - @Deprecated(since = "2024", forRemoval = true) - public TrapezoidProfile(Constraints constraints, State goal) { - this(constraints, goal, new State(0, 0)); - } - - /** - * Calculates the position and velocity for the profile at a time t where the current state is at - * time t = 0. - * - * @param t How long to advance from the current state toward the desired state. - * @return The position and velocity of the profile at time t. - * @deprecated Pass the desired and current state into calculate instead of constructing a new - * TrapezoidProfile with the desired and current state - */ - @Deprecated(since = "2024", forRemoval = true) - public State calculate(double t) { - if (m_newAPI) { - throw new RuntimeException("Cannot use new constructor with deprecated calculate()"); - } - State result = new State(m_current.position, m_current.velocity); - - if (t < m_endAccel) { - result.velocity += t * m_constraints.maxAcceleration; - result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; - } else if (t < m_endFullSpeed) { - result.velocity = m_constraints.maxVelocity; - result.position += - (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel - + m_constraints.maxVelocity * (t - m_endAccel); - } else if (t <= m_endDeccel) { - result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; - double timeLeft = m_endDeccel - t; - result.position = - m_goal.position - - (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; - } else { - result = m_goal; - } - - return direct(result); } /** diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 15b101dfe8..90c5cd973f 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -102,23 +102,6 @@ class TrapezoidProfile { * @param constraints The constraints on the profile, like maximum velocity. */ TrapezoidProfile(Constraints constraints); // NOLINT - - /** - * Constructs a TrapezoidProfile. - * - * @param constraints The constraints on the profile, like maximum velocity. - * @param goal The desired state when the profile is complete. - * @param initial The initial state (usually the current state). - * @deprecated Pass the desired and current state into calculate instead of - * constructing a new TrapezoidProfile with the desired and current state - */ - [[deprecated( - "Pass the desired and current state into calculate instead of " - "constructing a new TrapezoidProfile with the desired and current " - "state")]] - TrapezoidProfile(Constraints constraints, State goal, - State initial = State{Distance_t{0}, Velocity_t{0}}); - TrapezoidProfile(const TrapezoidProfile&) = default; TrapezoidProfile& operator=(const TrapezoidProfile&) = default; TrapezoidProfile(TrapezoidProfile&&) = default; @@ -204,8 +187,6 @@ class TrapezoidProfile { Constraints m_constraints; State m_current; - State m_goal; // TODO: remove - bool m_newAPI; // TODO: remove units::second_t m_endAccel; units::second_t m_endFullSpeed; diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc index c970a791d5..75e76d9eee 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc @@ -12,89 +12,8 @@ namespace frc { template TrapezoidProfile::TrapezoidProfile(Constraints constraints) - : m_constraints(constraints), m_newAPI(true) {} + : m_constraints(constraints) {} -template -TrapezoidProfile::TrapezoidProfile(Constraints constraints, - State goal, State initial) - : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1}, - m_constraints(constraints), - m_current(Direct(initial)), - m_goal(Direct(goal)), - m_newAPI(false) { - if (m_current.velocity > m_constraints.maxVelocity) { - m_current.velocity = m_constraints.maxVelocity; - } - - // Deal with a possibly truncated motion profile (with nonzero initial or - // final velocity) by calculating the parameters as if the profile began and - // ended at zero velocity - units::second_t cutoffBegin = - m_current.velocity / m_constraints.maxAcceleration; - Distance_t cutoffDistBegin = - cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; - - units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration; - Distance_t cutoffDistEnd = - cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; - - // Now we can calculate the parameters as if it was a full trapezoid instead - // of a truncated one - - Distance_t fullTrapezoidDist = - cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd; - units::second_t accelerationTime = - m_constraints.maxVelocity / m_constraints.maxAcceleration; - - Distance_t fullSpeedDist = - fullTrapezoidDist - - accelerationTime * accelerationTime * m_constraints.maxAcceleration; - - // Handle the case where the profile never reaches full speed - if (fullSpeedDist < Distance_t{0}) { - accelerationTime = - units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); - fullSpeedDist = Distance_t{0}; - } - - m_endAccel = accelerationTime - cutoffBegin; - m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; - m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; -} - -template -typename TrapezoidProfile::State -TrapezoidProfile::Calculate(units::second_t t) const { - if (m_newAPI) { - throw std::runtime_error( - "Cannot use new constructor with deprecated Calculate()"); - } - State result = m_current; - - if (t < m_endAccel) { - result.velocity += t * m_constraints.maxAcceleration; - result.position += - (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; - } else if (t < m_endFullSpeed) { - result.velocity = m_constraints.maxVelocity; - result.position += (m_current.velocity + - m_endAccel * m_constraints.maxAcceleration / 2.0) * - m_endAccel + - m_constraints.maxVelocity * (t - m_endAccel); - } else if (t <= m_endDeccel) { - result.velocity = - m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; - units::second_t timeLeft = m_endDeccel - t; - result.position = - m_goal.position - - (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * - timeLeft; - } else { - result = m_goal; - } - - return Direct(result); -} template typename TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t, State current,