A little refactoring onf waitForMotion