Табличная попытка. Оставил недодел.
- сделаны таблицы поисковые - 2 штуки угловая и зубовая.
- сделан задел - цикл angleSearch для поиска табличного угла.
=========================================================================
Когда находимся в состоянии стопа и введен угол (пока нет обмена, подразумевается, что вводится просто нужный угол для каждого цилиндра, т.е. не смещение а подсчитанный готовый угол) ищем ближайший к нему табличный угол.
По номеру табличного угла смотрим какой это зуб.

Подстройку определяем по скорости во время работы.
Сначала вычисляем подстройку вычитанием табличного угла из заданного.
Скорость делим на 12 - это будет значение скорости для пол угла - результат есть количество дискрем, которые будут означать точку.