Проверил в симуляторе - работает как надо. Там фишка в том, что Cycle ms в reg_KZR должно равняться времени цикла программы. Иначе, да, работает лишь бы как.