Create planPath

4/28/2014
Kevin's main function for path planning.
Requires:
Merge pull request #1 from kevinob99/patch-1

Create planPath