FINALLY FIXED calibrateServo() !!!!! Will add comments later