Updated to work for up-down servo as well