Smoothing Fix

Smoothing algorithm fixed. Added parameterized calls to RRT_Test_Script
for functions. Drawing call inside functions is commented. Test Script
is only drawing.