Fixed reversed servo initialization
More simplification