Added calculateServoPos for two modes. Added lockPosition struct, to be set on calibration and/or mode select.