initial implementation of servo control