diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index 938ca75..bba7696 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -98,11 +98,14 @@
   if (top_velocity > maximum_velocity_) {
     acceleration_time_ =
         (maximum_velocity_ - output_(1)) / maximum_acceleration_;
-    constant_time_ =
-        (distance_to_target + (goal_velocity * goal_velocity -
-                               maximum_velocity_ * maximum_velocity_) /
-                                  (2.0 * maximum_acceleration_)) /
-        maximum_velocity_;
+    constant_time_ = ((-0.5 * maximum_acceleration_ * acceleration_time_ *
+                           acceleration_time_ -
+                       output_(1) * acceleration_time_) +
+                      distance_to_target +
+                      (goal_velocity * goal_velocity -
+                       maximum_velocity_ * maximum_velocity_) /
+                          (2.0 * maximum_acceleration_)) /
+                     maximum_velocity_;
   } else {
     acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
   }
@@ -114,7 +117,12 @@
     acceleration_time_ = 0;
   }
 
-  deceleration_time_ = (goal_velocity - top_velocity) / deceleration_;
+  deceleration_time_ =
+      (goal_velocity - ::std::min(top_velocity, maximum_velocity_)) /
+      deceleration_;
+  if (constant_time_ <= 0) constant_time_ = 0;
+  if (deceleration_time_ <= 0) deceleration_time_ = 0;
+  if (acceleration_time_ <= 0) acceleration_time_ = 0;
 }
 
 }  // namespace aos::util
diff --git a/aos/util/trapezoid_profile_test.cc b/aos/util/trapezoid_profile_test.cc
index 92b9996..34dc35c 100644
--- a/aos/util/trapezoid_profile_test.cc
+++ b/aos/util/trapezoid_profile_test.cc
@@ -10,7 +10,7 @@
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
  protected:
-  TrapezoidProfileTest() : profile_(delta_time) {
+  TrapezoidProfileTest() : profile_(kDeltaTime) {
     position_.setZero();
     profile_.set_maximum_acceleration(0.75);
     profile_.set_maximum_velocity(1.75);
@@ -21,6 +21,16 @@
     position_ = profile_.Update(goal_position, goal_velocity);
   }
 
+  void RunFor(double goal_position, double goal_velocity,
+              std::chrono::nanoseconds duration) {
+    while (duration > std::chrono::nanoseconds(0)) {
+      position_ = profile_.Update(goal_position, goal_velocity);
+      duration -= kDeltaTime;
+    }
+
+    ASSERT_EQ(duration.count(), 0);
+  }
+
   const Eigen::Matrix<double, 2, 1> &position() { return position_; }
 
   TrapezoidProfile profile_;
@@ -40,13 +50,13 @@
   }
 
  private:
-  static constexpr ::std::chrono::nanoseconds delta_time =
+  static constexpr ::std::chrono::nanoseconds kDeltaTime =
       ::std::chrono::milliseconds(10);
 
   Eigen::Matrix<double, 2, 1> position_;
 };
 
-constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::delta_time;
+constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::kDeltaTime;
 
 TEST_F(TrapezoidProfileTest, ReachesGoal) {
   for (int i = 0; i < 450; ++i) {
@@ -55,7 +65,7 @@
   EXPECT_TRUE(At(3, 0));
 }
 
-// Tests that decresing the maximum velocity in the middle when it is already
+// Tests that decreasing the maximum velocity in the middle when it is already
 // moving faster than the new max is handled correctly.
 TEST_F(TrapezoidProfileTest, ContinousUnderVelChange) {
   profile_.set_maximum_velocity(1.75);
@@ -118,4 +128,19 @@
   EXPECT_EQ(position()(0), 1.0);
 }
 
+// Tests that we can move a goal without the trajectory teleporting.
+TEST_F(TrapezoidProfileTest, MoveGoal) {
+  profile_.set_maximum_acceleration(2.0);
+  profile_.set_maximum_velocity(2.0);
+
+  RunFor(5.0, 0, std::chrono::seconds(1));
+  EXPECT_TRUE(At(1.0, 2.0));
+  RunFor(5.0, 0, std::chrono::seconds(1));
+  EXPECT_TRUE(At(3.0, 2.0));
+  RunFor(3.5, 0, std::chrono::seconds(1));
+  EXPECT_TRUE(At(4.0, 0.0));
+  RunFor(3.5, 0, std::chrono::seconds(1));
+  EXPECT_TRUE(At(3.5, 0.0));
+}
+
 }  // namespace aos::util::testing