Moved WaitForMotors() into every navigation function so they do not need to be called explicitly