fix: Enforce positive end phase time and simplify end phase distance

nobleo · Mar 22, 2022 · ec82e90 · cesar-lopez-mar · Mar 22, 2022 · ec82e90
1 parent d5198be
commit ec82e90
Showing 1 changed file with 4 additions and 8 deletions.
diff --git a/src/controller.cpp b/src/controller.cpp
@@ -448,16 +448,12 @@ Controller::UpdateResult Controller::update(
   if (
     (current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) ||
     (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) {
-    t_end_phase_current = (target_end_x_vel - current_x_vel) / (-config_.target_x_decc);
-    d_end_phase = current_x_vel * t_end_phase_current -
-                  0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current +
-                  fabs(target_x_vel) * 2.0 * dt.toSec();
+    t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_decc);
   } else {
-    t_end_phase_current = (target_end_x_vel - current_x_vel) / (config_.target_x_acc);
-    d_end_phase = current_x_vel * t_end_phase_current +
-                  0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current +
-                  fabs(target_x_vel) * 2.0 * dt.toSec();
+    t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_acc);
   }
+  d_end_phase = (current_x_vel + target_end_x_vel) * 0.5 * t_end_phase_current +
+                target_x_vel * 2.0 * dt.toSec();
   ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current);
   ROS_DEBUG("d_end_phase: %f", d_end_phase);
   ROS_DEBUG("distance_to_goal: %f", distance_to_goal);