More servo changes. Cleaner implementation