Как временно отключить прерывания TIMER5?

У меня есть довольно сложная программа автономного робота, которая попадает в неприятности при выполнении управляемых поворотов. Я построил очень маленькую программу, чтобы проверить только повороты, и это прекрасно работает. Однако когда я переношу алгоритм поворота в основную программу, он ведет себя хаотично, и я подозреваю, что виновником является прерывание TIMER5 ISR, которое находится в основной программе, но не в тестовой программе.

Чтобы устранить неполадки, я добавил код ISR TIMER5 в тестовую программу, и я хочу иметь возможность включать и отключать ISR с помощью команд клавиатуры, чтобы увидеть, могу ли я вызвать аномальное поведение с включенным ISR и предотвратить его, отключив ISR.

Из чтения о прерываниях ТАЙМЕРА следует, что установка TIMSKx (TIMSK5 в моем случае) в 0 отключит ISR, а установка бита 0 в 1 (с использованием TIMSKx |= OCIExA) включит его. У меня есть процедура 'CheckForUserInput ()', которая позволяет мне вводить команды с клавиатуры, поэтому я добавил команду 'T' следующим образом:

switch (incomingByte)
{
case 0x54: //ASCII 'T'
case 0x74: //ASCII 's'
    Serial.println(F("Toggle TIMER5 Enable/Disable"));
    if (TIMSK5 == 0)
    {
        Serial.println(F("Enable TIMER5"));
        TIMSK5 |= OCIE5A;
        mySerial.printf("TIMSK5 = %x\n", TIMSK5);
    }
    else
    {
        Serial.println(F("Disable TIMER5"));
        TIMSK5 = 0;
        mySerial.printf("TIMSK5 = %x\n", TIMSK5);
    }
    break;

У меня есть светодиод, настроенный мигать каждый раз, когда срабатывает ISR, и я вижу, что могу надежно остановить ISR от срабатывания с помощью первого входа "T", но я не могу надежно перезапустить его со второй командой "T", хотя по распечаткам я могу сказать, что ISR работает надежно. правильный фрагмент кода выполнен, как в следующем выводе

Opening port
Port open

Checking for MPU6050 IMU at I2C Addr 0x69
MPU6050 connection successful
Initializing DMP...
Enabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...
MPU6050 Ready at 0.52 Sec
End of test - Stopping!
I received: 63
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Back to Auto Mode
S = Stop
F = Forward
R = Reverse

       Faster
    8
Left 4  5  6 Right
    2
       Slower
Got D
Got A
Got 74
Toggle TIMER5 Enable/Disable
Disable TIMER5
TIMSK5 = 0
Got D
Got A
Got 74
TEnabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...
MPU6050 Ready at 0.52 Sec
End of test - Stopping!
I received: 63
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Back to Auto Mode
S = Stop
F = Forward
R = Reverse

       Faster
    8
Left 4  5  6 Right
    2
       Slower
Got D
Got A
Got 74
Toggle TIMER5 Enable/Disable
Disable TIMER5
TIMSK5 = 0
Got D
Got A
Got 74
Toggle TIMER5 Enable/Disable
Enable TIMER5
TIMSK5 = 1
Got D
Got A

Я уверен, что делаю что-то не так в том, как я пытаюсь повторно включить TIMER5 ISR, но я действительно не вижу этого. У кого-нибудь есть подсказка?

ТИА,

Фрэнк