Added servo testing code