Init code based on servo-driver