Код ISR +, интегрирующий код подсчета с подпрограммой ISR

У меня есть раздел кода C для устройства PIC, он активирует 4 отдельных реле по заранее определенному шаблону, каждое из которых устанавливается индивидуально, подсчитывая время и частоту появления. Этот паттерн продолжается до бесконечности, но обнаружил, что время на стандартном delay_ms недостаточно точное. Я хочу преобразовать его в процедуру ISR, я прилагаю ниже код, который зависит от стандартной задержки для вашего прочтения, моя главная задача заключается в том, как абстрагировать этот код в ISR, поскольку я знаю, что не следует вставлять весь этот код в фактическую процедуру ISR, любой совет очень ценен.

__CONFIG(FOSC_XT & WDTE_OFF & PWRTE_OFF & MCLRE_OFF & CP_OFF & CPD_OFF &
        BOREN_OFF & CLKOUTEN_OFF & IESO_OFF & FCMEN_OFF);

//!!! use BORV_HI for latest PICC compiler
__CONFIG(WRT_OFF & PLLEN_OFF & STVREN_OFF & LVP_OFF);       

#define _XTAL_FREQ  4000000

unsigned long int Sample_Period =20;
unsigned long int Sample_Duration = 2;
unsigned long int Sample_ON = 0;
unsigned long int WriteTX_Period = 21;
unsigned long int WriteTX_Duration = 1; //  
unsigned long int WriteTX_ON = 0;
unsigned long int Depass_Period = 60; //every 7 days for depass event
unsigned long int Depass_Duration = 10 ; // depass for 120 seconds
unsigned long int Depass_ON = 0 ;
unsigned long int Depass_Counter = 0;
unsigned long int Sample_Counter = 0;
unsigned long int WriteTX_Counter = 0;
unsigned long int count = 0;
unsigned char input;
char data = 1;

void SkipLine(void){
    printf("\n");
    printf("\r");
}

void main() {
    INTCON = 0;                 // disable interrupts.
    ANSELA = 0x00;              // all digital IO
    ANSELB = 0x00;              // all digital IO
    TRISA = 0b00000000;         // Configure PORTA as output 
    PORTA = 0b00000111;         // Initialize PORTA, all load relays are off.
    ADCON0 = 7;                 // disables ADC
    CM1CON0 = 7;                // Disable analog comparators
    TRISB = 0b10111001;         // all outputs bar RB1/RX pin, and RB7 (Prog'd)

    init_comms();

    printf("\n");
    printf("\r");
    printf("*******************************");
    printf("\n");
    printf("\r");
    printf("*METROL RELAY CONTROLLER MK1.0*");
    printf("\n");
    printf("\r");
    printf("*******************************");
    printf("\n");
    printf("\r");

    printf("Default timings are :");
    printf("\n");
    printf("\r");
    printf("Sample Period = ");
    printf("%d seconds", Sample_Period);
    printf("\n");
    printf("\r");

    printf("\n");
    printf("\r");
    printf("Sample Duration = ");
    printf("%d seconds", Sample_Duration);
    printf("\n");
    printf("\r");

    printf("\n");
    printf("\r");
    printf("WriteTX Period = ");
    printf("%d seconds", WriteTX_Period);
    printf("\n");
    printf("\r");

    printf("\n");
    printf("\r");
    printf("WriteTX Duration = ");
    printf("%d seconds", WriteTX_Duration);
    SkipLine;

    printf("\n");
    printf("\r");
    printf("Depassivation Period (Days)= ");
    printf("%d Days", Depass_Period);
    printf("\n");
    printf("\r");
    printf("\n");
    printf("\r");
    printf("Depassivation Duration  = ");
    printf("%d seconds", Depass_Duration);
    printf("\n");
    printf("\r");

    if (RB7 == 1)
    {
        printf("requires set up");

        printf("\n");
        printf("\r");
        printf("\n");
        printf("\r");

        printf("Enter value for Sample Period in minutes <0-255> ");
        printf("\n");
        printf("\r");

        char str[50];

        printf("Enter a string : ");
        gets(str);

        printf("You entered: %s", str);
        int SamplePeriodVal;
        SamplePeriodVal = atoi(str);
        printf("Sample Period Value entered = %d\n", SamplePeriodVal);
    }

    printf("\n");
    printf("\r");
    printf("system already configured");
    printf("\n");
    printf("\r");

    unsigned int Sample_Period_Units;
    Sample_Period_Units = EEPROM_READ(0x00);
    printf("sample value held in first eeprom address 0x00 is %d", EEPROM_READ(0x00));

    printf("\n");
    printf("\r");
    printf("load profile starting.....");
    printf("\n");
    printf("\r");

    while (1) {
        printf("\n");
        printf("\r");
        printf("test!");
        printf("\n");
        printf("\r");

        __delay_ms(990);

        if (Sample_Counter >= Sample_Period){
            PORTA = 0b00000110; //set Sample relay ON

            Sample_ON++;

            if (Sample_ON > Sample_Duration){
                Sample_ON = 0;
                Sample_Counter = 0;
                PORTA = 0b00000111;

            }
        }

        if (WriteTX_Counter >= WriteTX_Period){
            PORTA = 0b00000100; //set Write relay ON

            WriteTX_ON++;

            if (WriteTX_ON > WriteTX_Duration){
                WriteTX_ON = 0;
                WriteTX_Counter = 0;
                PORTA = 0b00000111;

            }
        }

        if (Depass_Counter >= Depass_Period){
            PORTA = 0b00000011; //set Depass relay ON


            Depass_ON++;

            if (Depass_ON > Depass_Duration){
                Depass_ON = 0;
                Depass_Counter = 0;
                PORTA = 0b00000111;

            }
        }

        Sample_Counter++;
        WriteTX_Counter++;
        Depass_Counter++;
        count++;            // increment total count for system

        printf("\n");
        printf("\r");

        int SampleAct;
        SampleAct = RB3;
        printf("Port B sample value =%d  ", SampleAct);

        printf("\r");
        printf("\n");

        int WriteTXAct;
        WriteTXAct = RB4;
        printf("Port B WriteTX value =%d  ", WriteTXAct);

        printf("\r");
        printf("\n");

        int DepassAct;
        DepassAct = RB5;
        printf("Port B Depass value =%d  ", DepassAct);

        printf("\r");
        printf("\n");
        printf("%ld", count);

        int PortB_Val;

        PortB_Val = PORTB & 0b00111000;

        switch (PortB_Val)
        {

        case 0x28:
            RB6 = RB6;
            printf("\n");
            printf("\r");
            printf("Sample+Depass error");
            break;

        case 0x30:
            RB6 = RB6;
            printf("\n");
            printf("\r");
            printf("Write+Depass error");
            break;

        case 0x38:
            RB6 = RB6;
            printf("\n");
            printf("\r");
            printf("Write+Sample+Depass error");
            break;

        default:
            RB6 = !RB6;
        }
    }
}