Added servo control. Testing purposes only
started the template