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 448c8384cf..7e7bbdcff8 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 @@ -43,7 +43,7 @@ public class TrapezoidProfile { private int m_direction; private final Constraints m_constraints; - private State m_current; + private State m_current = new State(); private double m_endAccel; private double m_endFullSpeed; @@ -187,7 +187,8 @@ public class TrapezoidProfile { * Returns the time left until a target distance in the profile is reached. * * @param target The target distance. - * @return The time left until a target distance in the profile is reached. + * @return The time left until a target distance in the profile is reached, or zero if no goal was + * set. */ public double timeLeftUntil(double target) { double position = m_current.position * m_direction; @@ -253,7 +254,7 @@ public class TrapezoidProfile { /** * Returns the total time the profile takes to reach the goal. * - * @return The total time the profile takes to reach the goal. + * @return The total time the profile takes to reach the goal, or zero if no goal was set. */ public double totalTime() { return m_endDecel; diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 9dd82f4eee..72224fda83 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -200,7 +200,8 @@ class TrapezoidProfile { * Returns the time left until a target distance in the profile is reached. * * @param target The target distance. - * @return The time left until a target distance in the profile is reached. + * @return The time left until a target distance in the profile is reached, or + * zero if no goal was set. */ constexpr units::second_t TimeLeftUntil(Distance_t target) const { Distance_t position = m_current.position * m_direction; @@ -271,7 +272,8 @@ class TrapezoidProfile { /** * Returns the total time the profile takes to reach the goal. * - * @return The total time the profile takes to reach the goal. + * @return The total time the profile takes to reach the goal, or zero if no + * goal was set. */ constexpr units::second_t TotalTime() const { return m_endDecel; } @@ -311,14 +313,14 @@ class TrapezoidProfile { } // The direction of the profile, either 1 for forwards or -1 for inverted - int m_direction; + int m_direction = 1; Constraints m_constraints; State m_current; - units::second_t m_endAccel; - units::second_t m_endFullSpeed; - units::second_t m_endDecel; + units::second_t m_endAccel = 0_s; + units::second_t m_endFullSpeed = 0_s; + units::second_t m_endDecel = 0_s; }; } // namespace frc 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 64dfa4784d..72b730420f 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 @@ -246,4 +246,11 @@ class TrapezoidProfileTest { } } } + + @Test + void initalizationOfCurrentState() { + var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1)); + assertNear(profile.timeLeftUntil(0), 0, 1e-10); + assertNear(profile.totalTime(), 0, 1e-10); + } } diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp index 39cbc3b0ea..e83da634b6 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp @@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) { } } } + +TEST(TrapezoidProfileTest, InitalizationOfCurrentState) { + frc::TrapezoidProfile::Constraints constraints{1_mps, 1_mps_sq}; + frc::TrapezoidProfile profile{constraints}; + EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s); + EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s); +}