Initial Servo functions

Basic initialization routines.