Adding servo control core and support software