Adding invertTheta function for those using counter-clockwise servos