Replaced the setArrived methods to the last waypointchecks.
Merge pull request #92 from jeroen940/SmallCraneFix

Replaced the setArrived methods to the last waypointchecks.