Refactored code to use motion control functions