change get_current_position from motion control, and code refactor