Finding shortest path in short time --> working version --> using 'A Star' on only adjacent nodes, not PQ
Merge branch 'master' of https://github.com/PrincetonRoboticsClub/BrownMaze2013