Для подобных программ существует язык SFC, который позволяет буквально "рисовать" алгоритм в программе и в дальнейшем удобен при отладке и её модификации.
Таймеры, состояния - всё уже есть внутри. Вариант упрощённого SFC с вставками на ST открывает очень широкие возможности.
Конечно, требуется некоторое время на изучение и "вхождение", но я считаю, оно того стоит.
Если интересно - посмотрите например
Ответить с цитированием