Fixed bugged out guys, added code for pathfollowing
Merge pull request #2 from mve9302/master

Fixed bugged out guys, added code for pathfollowing