Arduino code refactor to properly reset motors