updated servo routine