Added servo initialization