Methods and systems of correcting chromatic focal shift may include measuring a chromatic focal shift in focal points of a lens between first and second wavelengths of light passing through the lens. They also include detecting the first wavelength of light from a sample spaced at a first distance between the sample and the lens that corresponds to a first focal point for the first wavelength of light. They further include adjusting the sample and the lens to a second distance with a piezoelectric actuator. The second distance may be determined using the measurement of the chromatic focal shift between the first and second wavelengths of light passing through the lens. They additionally include detecting a second wavelength of light from the sample spaced at the second distance between the sample and the lens, where the second distance corresponds to a second focal point for the second wavelength of light.