Este problema se puede resolver poniendo la lógica que lo resuelve en las interrupciones y en el main simplemente inicializando y dejando un bucle infinito que no hace nada.
Lo que no deberias habilitar las interrupciones en la interrupción. Si te entendi bien cada rutina ejecuta rápidamente y retorna el control por lo cual no hay que habilitarlas.
La rutina tiempo está dedicada a la lógica que tu quieras poner.
Saludos,
Gustavo