Arduino Zero - управление шаговым двигателем (прерывание)?

У меня есть плата контроллера шагового двигателя, которая должна пульсировать каждые 40 мкс или около того.

У меня также есть датчики I2C и последовательная запись в преформу.

В настоящее время я запускаю вызов обновления в тесном цикле, но датчики I2C и последовательные записи вызывают значительные задержки, из-за которых моторы заикаются и останавливаются.

Я хотел бы использовать ISR для периодического вызова метода обновления мотора.

Однако я ни за что не могу разобраться с этим таймером.

Вот моя текущая попытка:

void TC4_Handler() {
  Stepper::updateAll();
}

void setup() {

  // Таймеры
  // Настраиваем общие часы (GCLK4), используемые для тактирования таймеров
  REG_GCLK_GENDIV = GCLK_GENDIV_DIV(1) |          // Делим источник тактовой частоты 48 МГц на делитель 1: 48 МГц/1=48 МГц
                    GCLK_GENDIV_ID(4);            // Выбор общих часов (GCLK) 4
  while (GCLK->STATUS.bit.SYNCBUSY);              // Ждем синхронизации

  REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC |           // Установите рабочий цикл на 50/50 ВЫСОКИЙ/НИЗКИЙ
                     GCLK_GENCTRL_GENEN |         // Включить GCLK4
                     GCLK_GENCTRL_SRC_DFLL48M |   // Установите источник тактовой частоты 48 МГц
                     GCLK_GENCTRL_ID(4);          // Выбираем GCLK4
  while (GCLK->STATUS.bit.SYNCBUSY);              // Ждем синхронизации

  // Подача GCLK4 на TC4 и TC5
  REG_GCLK_CLKCTRL = GCLK_CLKCTRL_CLKEN |         // Включить GCLK4 для TC4 и TC5
                     GCLK_CLKCTRL_GEN_GCLK4 |     // Выбираем GCLK4
                     GCLK_CLKCTRL_ID_TC4_TC5;     // Подача GCLK4 на TC4 и TC5
  while (GCLK->STATUS.bit.SYNCBUSY);              // Ждем синхронизации

  REG_TC4_COUNT16_CC0 = 0x000F;                      // Установить регистр TC4 CC0 в качестве значения TOP в режиме совпадения частот
  while (TC4->COUNT16.STATUS.bit.SYNCBUSY);       // Ждем синхронизации

  //NVIC_DisableIRQ(TC4_IRQn);
  //NVIC_ClearPendingIRQ(TC4_IRQn);
  NVIC_SetPriority(TC4_IRQn, 0);    // Установите приоритет Nested Vector Interrupt Controller (NVIC) для TC4 на 0 (самый высокий)
  NVIC_EnableIRQ(TC4_IRQn);         // Подключаем TC4 к вложенному контроллеру векторных прерываний (NVIC)

  REG_TC4_INTFLAG |= TC_INTFLAG_OVF;              // Сбрасываем флаги прерывания
  REG_TC4_INTENSET = TC_INTENSET_OVF;             // Разрешить прерывания TC4
  // REG_TC4_INTENCLR = TC_INTENCLR_OVF; // Отключить прерывания TC4

  REG_TC4_CTRLA |= TC_CTRLA_PRESCALER_DIV1024 |   // Установите прескалер на 1024, 48 МГц/1024 = 46,875 кГц
                   TC_CTRLA_WAVEGEN_MFRQ |        // Переводим таймер TC4 в режим согласования частоты (MFRQ)
                   TC_CTRLA_ENABLE;               // Включить TC4
  while (TC4->COUNT16.STATUS.bit.SYNCBUSY);       // Ждем синхронизации
  // ...
}