going back to original servo code