1-ый этап:
измеренный период управляемого вала подгоняется к постоянновращающемуся. Происходит в три подэтапа с разными допустимыми диапазонами ошибок и с разными корректирующими сигналами чтобы быстрее выйти на режим.

2-ой этап:
Определение разницы сигнала от управляемого вала к другому, и соответвующаяя коррекция.

В принципе работает, но попробую прицепить ПИД как вы указали ибо делать надо по уму