Revert "last fix was incorrect"

This reverts commit ba465aa1691d6155fa9d6b3c6fe60a069491bf54.
Revert "Revert "last fix was incorrect""

This reverts commit dec56107c9cf0f794952171a121b0c8972bf0e85.
Revert "Ugliest code in the history of mankind, also tries to plan a path with a margin"

This reverts commit 1f2b1727df085c98566947f20f98d0a9f7b92c76.
Merge pull request #23 from hasofanten/master

Removed margin thingy