Fixed servo issues