Столкновение прерываний таймера

Я работаю с MEGA2560, пытаясь реализовать скоростные рампы для трех степперов одновременно. Цель состоит в том, чтобы двигаться в разные позиции, применяя ускорения и замедления, одновременно завершая их все.

Чтобы сделать это, исходя из расстояний, на которые должен двигаться каждый из двигателей, я вычисляю количество шагов, которые нужно сделать, и скорости для каждого из этих шагов, чтобы гарантировать, что все двигатели закончат работу одновременно. Для этого я использую таймеры 3, 4 и 5 в режиме сравнения. Таким образом, при каждом перерыве я заставляю двигатели делать один шаг и после этого устанавливаю новое значение сравнения для таймера, чтобы достичь новой скорости движения.

Моя проблема в том, что в какой-то момент мои прерывания терпят крах, и один из двигателей перестает работать. Это происходит только при одном условии: когда двигатель 1 должен сделать 18 шагов, а двигатель 2-34. В этом случае оба двигателя 1 и 3 заканчивают работу, но двигатель 2 застревает и больше не движется. Хотя, если его мотор 1, который должен сделать 34 шага, и мотор 2, 18, то все работает. Двигатель 1 связан с таймером 3, а двигатель 2-с таймером 4. Это наводит меня на мысль, что это должно быть связано с приоритетами прерываний.

В следующем коде вы можете увидеть мои прерывания:

ISR(TIMER3_COMPA_vect) {
    m1_step();
    m1_steps_done++;
    m1_configure_vel();
}
ISR(TIMER4_COMPA_vect) {
    m2_step();
    m2_steps_done++;
    m2_configure_vel();
}
ISR(TIMER5_COMPA_vect) {
    m3_step();
    m3_steps_done++;
    m3_configure_vel();
} 

В то время как в конфигурации скорости я имею:

void m1_configure_vel() {

  if (m1_steps_done < m1_acceleration_steps) {
    m1_temp_vel = m1_temp_vel + acceleration;
  }
  else if (m1_steps_done <= (m1_steps - m1_acceleration_steps)) {
    m1_temp_vel = m1_temp_vel;
  }
  else if (m1_steps_done < m1_steps) {
    m1_temp_vel = m1_temp_vel - acceleration;
  }
  else {
    m1_temp_vel = 0;
    m1_finished = true;
  }
  if (m1_temp_vel <=0){
    m1_temp_vel = 0;
  }

  OCR3A = frequency_to_compare_value(m1_temp_vel);
}

Как вы можете видеть, все, что я делаю здесь, зависит от шагов, выполняемых двигателем, и количества шагов, которые нужно сделать, я увеличиваю, поддерживаю или уменьшаю скорость двигателя и устанавливаю значение сравнения для таймера.

Все дело в том, что этот код работает нормально, пандусы рассчитаны правильно, а двигатели двигаются правильно. Проблема в том, что при условии, которое я объяснил ранее, прерывания могут сталкиваться из-за их приоритетов, и один из них просто перестает происходить.

Мне было интересно, имел ли кто-нибудь представление или сталкивался с чем-то подобным раньше.