Так у Вас все есть, вот четко по алгоритму и действуйте. Просто код старайтесь дедать максимально простым и учитывайте, что необходимы задержки при отработке команд до чтения состояния, причем величина задержек прямопропорциональна инертности исполнительных элементов.
__________________
|