Refactored servo init