Jan28 Fixed servo issues, seems to have messed up algorithm