Подпрограмма, куда основная программа входит, делает итерацию и переключается на след. подпрограмму, и так по кругу...
Если интересно, могу дать.

void Command::IfChange()
{
 char temp;
 temp = Input; // состояние входов
 temp ^= 0x2c; // инверсные сигналы датчиков
 switch (step) // состояние цифрового автомата
  {
   case  0: if(time) break; // задержка после включения
            if((temp&0x0c)==0x0c) // оба датчика одновременно
              { K1 = 1; break; } // реле неисправность
             else K1 = 0; // реле неисправность в исходное
            if(temp&8) { step = 20; break; } // датчик "ВКЛ"
            if(temp&4) { step = 10; break; } // датчик "ОТКЛ"
            step++; break; // нет сигналов с датчиков, среднее положение
   case  1: if(temp&0x10) { step = 11; break; } // ток есть, довключить
            temp = ReadCommand(); // команда перед выключением
            if(temp==SetON) { step = 11; break; } // включение
            if(temp==SetOFF) { step = 21; break; } // отключение
            step = 21; break; // тока нет, довыключить
//****** ждем команду "включить разъединитель" ******
   case 10: OK1 = 0; // снять запрет соседнему контроллеру
            if(temp&8) { step = 20; break; } // сработал концевик
            if((temp&1)==0) break; // ждем команду "включить"
            if((temp&3)==3) { timeK2 = 3000; break; } // обе кнопки одновременно
            if(temp&0x20) { timeK2 = 3000; break; } // запрет от соседнего
            step++; break;
//************* включение двигателя *****************
   case 11: WriteCommand(SetON); // запомнить команду в EEPROM
            OK1 = 1; // запрет соседнему контроллеру
            COM1A = 2; // вкл. ШИМ
            Direct = 1; // направление двигателя
            PWM_Enable = 1; // включить двигатель
            time = 1000; // 1 сек. не контролируем ток двигателя
            step++; break;
   case 12: if(temp&8) { step++; break; } // сработал концевик
            if(temp&0x20) { step = 21; break; } // запрет от соседнего
            if(time) break; // 1 сек не контролируем ток двигателя
            time = 7000; // 7 сек на включение двигателя
            step++; break;
   case 13: if(temp&0x20) { step = 21; break; } // запрет от соседнего
            if(temp&8) { K1 = 0; step++; break; } // сработал концевик
            if((time==0)||Comp) step = 16; // ток двигателя или время
            break;
//************* выключение двигателя ****************
   case 14: WriteCommand(SetNone); // запомнить команду в EEPROM
            COM1A = 0; // откл. ШИМ
            PWM_Enable = 0; // выключить двигатель
            time = 1000; // 1 сек. на остановку двигателя
            step++; break;
   case 15: if(time) break; // 1 сек. на остановку двигателя
            step = 20; break; // на команду выключения
//************* не сработал концевик ****************
   case 16: K1 = 1; // реле неисправность
            PWM_Enable = 0; // выключить двигатель
            time = 1000; // 1 сек. на остановку двигателя
            step++; break;
   case 17: if(time) break; // 1 сек. на остановку двигателя
            Direct = 0; // направление двигателя
            PWM_Enable = 1; // включить двигатель
            time = 200; // 0,2 сек. обратный ход
            step++; break;
   case 18: if(time) break; // 0,2 сек. обратный ход
            step = 14; break; // выключение двигателя
//***** ждем команду "выключить разъединитель" ******
   case 20: OK1 = 1; // запрет соседнему контроллеру
            if(temp&4) { step = 10; break; } // сработал концевик
            if((temp&2)==0) break; // ждем команду "выключить"
            if((temp&3)==3) { timeK2 = 3000; break; } // обе кнопки одновременно
            if(temp&0x30) // запрет по току или от соседнего
             { 
              timeK2 = 3000; // включить K2 на 3 сек.
              break;
             }
            step++; break;
//************* включение двигателя *****************
   case 21: WriteCommand(SetOFF); // запомнить команду в EEPROM
            COM1A = 2; // вкл. ШИМ
            Direct = 0; // направление двигателя
            PWM_Enable = 1; // включить двигатель
            time = 1000; // 1 сек. не контролируем ток двигателя
            step++; break;
   case 22: if(temp&0x10) { step = 11; break; } // появился ток, включить обратно
            if(temp&4) { step++; break; } // сработал концевик
            if(time) break; // 1 сек не контролируем ток двигателя
            time = 7000; // 7 сек на включение двигателя
            step++; break;
   case 23: if(temp&0x10) { step = 11; break; } // появился ток, включить обратно
            if(temp&4) { K1 = 0; step++; break; } // сработал концевик
            if((time==0)||Comp) // ток двигателя или время
            step = 26; break;
//************* выключение двигателя ****************
   case 24: WriteCommand(SetNone); // запомнить команду в EEPROM
            COM1A = 0; // откл. ШИМ
            PWM_Enable = 0; // выключить двигатель
            time = 1000; // 1 сек. на остановку двигателя
            step++; break;
   case 25: if(time) break; // 1 сек. на остановку двигателя
            step = 10; break; // на команду включения
//************* не сработал концевик ****************
   case 26: K1 = 1; // реле неисправность
            PWM_Enable = 0; // выключить двигатель
            time = 1000; // 1 сек. на остановку двигателя
            step++; break;
   case 27: if(time) break; // 1 сек. на остановку двигателя
            Direct = 1; // направление двигателя
            PWM_Enable = 1; // включить двигатель
            time = 200; // 0,2 сек. обратный ход
            step++; break;
   case 28: if(time) break; // 0,2 сек. обратный ход
            step = 24; break; // выключение двигателя
//********* непредусмотренные состояния *************
   default: time = 1000; step = 0; break;
  }
 if(flag>=100) // пора сообщать сторожевой собаке
  {
   SendEvent(Complete,2); // генерить событие для WatchDog
   flag = 0; // исходное
  }
}