From 0de06534d22e22d84b4fe65ad973525e16e312c0 Mon Sep 17 00:00:00 2001
From: Yilun Chen <chenyilun12@gmail.com>
Date: Fri, 17 Jan 2020 19:17:28 -0500
Subject: [PATCH] cleanup in dynamic window approach

---
 .../DynamicWindowApproach/dynamic_window_approach.py      | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index 92fd08bb..9e9d34d7 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -22,7 +22,7 @@ def dwa_control(x, config, goal, ob):
 
     dw = calc_dynamic_window(x, config)
 
-    u, trajectory = calc_final_input(x, dw, config, goal, ob)
+    u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
 
     return u, trajectory
 
@@ -77,9 +77,9 @@ def motion(x, u, dt):
     motion model
     """
 
-    x[2] += u[1] * dt
     x[0] += u[0] * math.cos(x[2]) * dt
     x[1] += u[0] * math.sin(x[2]) * dt
+    x[2] += u[1] * dt
     x[3] = u[0]
     x[4] = u[1]
 
@@ -101,7 +101,7 @@ def calc_dynamic_window(x, config):
           x[4] - config.max_dyawrate * config.dt,
           x[4] + config.max_dyawrate * config.dt]
 
-    #  [vmin,vmax, yaw_rate min, yaw_rate max]
+    #  [vmin, vmax, yaw_rate min, yaw_rate max]
     dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
           max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
 
@@ -124,7 +124,7 @@ def predict_trajectory(x_init, v, y, config):
     return traj
 
 
-def calc_final_input(x, dw, config, goal, ob):
+def calc_control_and_trajectory(x, dw, config, goal, ob):
     """
     calculation final input with dynamic window
     """