diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py
index 089636d1..3e823810 100644
--- a/tools/Motion Planning/FIR_Planner.py	
+++ b/tools/Motion Planning/FIR_Planner.py	
@@ -13,7 +13,7 @@ import random
 # vr, ar and dr                 Reached values of velocity and acceleration
 # Tj , Tja, Tjv and Tjd         Length of the constant jerk stages (FIR filter time)
 
-def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
+def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax):
 
     dX = Xf - Xi    # Distance to travel
     s = np.sign(dX)   # Sign
@@ -21,9 +21,6 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
     Ar = s*Amax     # Maximum Acceleration (signed)
     Dr = -s*Dmax    # Maximum Deceleration (signed)
     Vr = s*Vmax     # Maximum Velocity (signed)
-
-    if(s*Vi > s*Vr):
-        Ar = -s*Amax
     
     Ta = (Vr - Vi)/Ar   # Acceleration Time
     Td = (-Vr)/Dr   # Deceleration Time
@@ -32,7 +29,7 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
     dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0
 
     ## Short move handling
-    if s*dXmin > s*dX:  
+    if s*dXmin > s*dX:
         Vr = s*math.sqrt((-(Vi**2 / Ar)-(2*dX))/(1/Dr - 1/Ar))
         Ta = max(0, (Vr - Vi)/Ar)
         Tv = 0
@@ -40,7 +37,6 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
     else:
         Tv = (dX - dXmin)/Vr    # non-short move, coast time at constant v
 
-
     ## We've computed Ta, Tv, Td, and Vr.  Time to produce a trajectory
     # Create the time series and preallocate the position, velocity, and acceleration arrays
     t_traj = np.linspace(0, Ta+Tv+Td, 10000)
@@ -55,10 +51,10 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
 
     for i in range(len(t_traj)):
         t = t_traj[i]
-        if(t <= 0): # Initial conditions
+        if(t < 0): # Initial conditions
             y[i] = Xi
             yd[i] = Vi
-            ydd[i] = Ai
+            ydd[i] = Ar
         elif(t <= Ta):  # Acceleration
             y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi
             yd[i] = (Ar * t) + Vi
@@ -75,42 +71,52 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax):
     return (y, yd, ydd, t_traj)
 
 
-#(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022)
 numRows = 2
 numCols = 4
 fig, axes = plt.subplots(numRows, numCols, sharey='all')
 random.seed()
-for x in range(8):
+for x in range(numRows*numCols):
 
     Vmax = random.uniform(0.1, 20)
-    Amax = random.uniform(0.1, 40)
+    Amax = random.uniform(0.1, 10)
 
-    Xi = random.uniform(-100.0, 100.0)
-    Vi = random.uniform(-Vmax, Vmax)
     Xf = random.uniform(-100.0, 100.0)
+    Xi = random.uniform(-100.0, 100.0)
+    maxVi = math.sqrt(abs(Xf-Xi)*2*Amax)
+    Vi = random.uniform(-maxVi, maxVi)
 
-    (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax)
 
-    if(abs(Xf-Y[-1]) > 0.0001):
+    (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Amax)
+
+    if(abs(Xf-float(Y[-1])) > 0.0001):
         print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1]))
-        plt.plot(t, Y)
-        plt.plot(t, Yd)
-        plt.plot(t, Ydd)
-        plt.show()
+        # plt.figure()
+        # plt.subplot(2,1,1)
+        # plt.plot(t, Y)
+        # plt.plot(t, Yd)
+        # plt.plot(t[-1], Xf, 'b*')
+        # plt.plot(t[-1], 0, 'r*')
+
+        # plt.subplot(2,1,2)
+        # plt.plot(t, Ydd)
+
+        # plt.show()
     
     elif(abs(Yd[-1]) > 0.0001):
         print("Bad final Velocity: ", Yd[-1])
-        plt.plot(t, Y)
-        plt.plot(t, Yd)
-        plt.plot(t, Ydd)
-        plt.show()
-
-    else:
-        print("Position Error: {:.6f}\tVelocity Error: {:.6f}".format(abs(Xf-Y[-1]),abs(Yd[-1])))
+        # plt.figure()
+        # plt.plot(t, Y)
+        # plt.plot(t, Yd)
+        # # plt.plot(t, Ydd)
+        # plt.show()
 
     axes[int(x/numCols), x%numCols].plot(t, Y)
     axes[int(x/numCols), x%numCols].plot(t, Yd)
     axes[int(x/numCols), x%numCols].plot(t, Ydd)
-    axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f}  Xf: {:.3f}'.format(Xi, Xf))
+    axes[int(x/numCols), x%numCols].plot(t[-1], Xf, 'b*')
+    axes[int(x/numCols), x%numCols].plot(t[-1], 0, 'r*')
+    dX = abs(Xf - Y[-1])
+    dV = abs(0 - Yd[-1])
+    axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f}  Xf: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xi, Xf, dX, dV))
 
 plt.show()