Route following logic working
slightly better debug messages