autoleveling software

I am building a motorized microscope stage, with mototized focus (i.e. XYZ). Microscope stages are supposed to be very flat, so the image does not go out of focus as you move around the slide.

A 3D printed microscope cannot be a flat as a precision machined metal stage, so I am looking into using an autoleveling software approach, such as used on the Mini and TAZ.

I would like to find the equations/software for stage leveling, which I will use for the focus control on my stage.

Can anyone point me in the right direction?