Create planPath

4/28/2014
Kevin's main function for path planning.
Requires: