diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py
index dc9710f1..b594f188 100644
--- a/tools/Motion Planning/Planner.py	
+++ b/tools/Motion Planning/Planner.py	
@@ -52,26 +52,24 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001):
             yd[i] = Vi
             ydd[i] = Ai
         elif(t <= Ta):  # Acceleration
-            y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT)
-            yd[i] = yd[i-1] + ydd[i-1]*dT
+            y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi
+            yd[i] = (Ar * t) + Vi
             ydd[i] = Ar
         elif(t <= Ta+Tv):   # Coasting
-            y[i] = y[i-1] + yd[i-1] * dT
-            yd[i] = yd[i-1]
+            y[i] = ((Ar*Ta*Ta) / 2) + (Vi * Ta) + (Vr * (t - Ta)) + Xi
+            yd[i] = Ar*Ta + Vi
             ydd[i] = 0
-        elif(t < Ta+Tv+Td): # Deceleration
-            y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT)
-            yd[i] = yd[i-1] + ydd[i-1]*dT
+        elif(t <= Ta+Tv+Td): # Deceleration
+            y[i] = ((Ar*Ta*Ta) / 2) + (Vi * Ta) + (Vr * (t - Ta)) + Xi + Dr*(t - Ta - Tv)*(t - Ta - Tv)/2
+            yd[i] = Ar*Ta + Vi + Dr*(t - Ta - Tv)
             ydd[i] = Dr
-        else:   # Final conditions
-            y[i] = Xf
-            yd[i] = Vf
-            ydd[i] = Af
+
     
     return (y, yd, ydd, t_traj)
 
 
-(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 0, 0, 10, 20, 20)
+(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 4, 0, 0, 2, 5, 5)
+print("Y: ",Y[len(Y)-1],"\nYd: ",Yd[len(Yd)-1])
 plt.plot(t, Y)
 plt.plot(t, Yd)
 plt.plot(t, Ydd)