Я пробовал отладить подобную систему управления, только в меню ничего задавать не нужно было, само всё калибровалось и работало по модели крана. Ничем хорошим это не закончилось, ошибки были слишком велики.
Буду ждать, когда вы опробуете свой алгоритм, может у вас получится лучше