added stops at heading changes
optimised drive controlling