Merge pull request #2 from RoboGroup3/master

Updates from Robot
 Some changes, fixed the blocking path navigation
Merge pull request #21 from JohanJohansson/master

Upates