Add servoReadValue function