Так выглядит программа для контроллера (функциональная диаграмма) реализующая указанный алгоритм. Программа проверена на симуляторе.
Процесс программирования сводится к рисованию таких вот схем.
Представленное решение - не единственно возможное - одну и ту же задачу можно решить несколькими способами.
Прилагаю исходный файл диаграммы - можно скачать, поиграть с ней и модифицировать, если кто захочет.