improve drive commands

- timeouts
- return if path blocked
- don't emergency-stop if close to destination