Improved motor functionality and added velocity direction