servo movement tuning up

-removed ^-1 from formula
-made changes to servo if statement