Ugliest code in the history of mankind, also tries to plan a path with a margin
Merge pull request #22 from hasofanten/master

Ugliest code in the history of mankind, also tries to plan a path with a...