The article describes a homegrown embedded measuring system for the shape inspection of optical parts that can be used in the ultra-­precision machines designed for diamond turning. The control system includes the contact and contactless sensors, a low-coherence interferometer, a signal converter and software that allows calculating any shape deviations of the spherical and aspherical surfaces and adjusting the CNC code to eliminate systematic shape deviations. The examples of using a contact sensor to increase the P-V shape accuracy of aspherical germanium lenses with a diameter of 90 mm from 1.4 μm to 100 nm after the systematic shape deviation compensation are provided. The possible application of a contactless sensor included in the measuring system to adjust an ultra-­precision machine, as well as to control flat and cylindrical surfaces are described.