drive on for non waiting point wait situations
Support for multiple workTools in mode 6
lower distance to change

- more accurate following of waypoints
closes #88