1. Используйте функцию SEL или функции выбора IF, Case....
2. Увеличьте мин. время цикла до 2-3 мс. На производстве S-400 работают с циклом 15-17мс, временами до 30мс. Если время критично выводите скоростные задачи на сопроцессор.
Скорее всего у вас в программе ошибки. Я в вашей программе сложных задач для сопроцессора не вижу.