Прошивка Marlin. Задача организовать таймер для подогрева стола

Конкретно необходимо постоянно с каким то периодом включать выключать подогрев стола. Итак нашёл вот эту часть когда.

#else // !PIDTEMPBED && !BED_LIMIT_SWITCHING

После неё сделал проверку

  const millis_t counter_ms= millis();
  if (ELAPSED(counter_ms,LimitT)){

    if (Turn==1){
      Turn=0;
      temp_bed.soft_pwm_amount = temp_bed.celsius < temp_bed.target ? MAX_BED_POWER >> 1 : 0;
      LimitT=counter_ms+3000UL;
    }else{
      Turn=1;
      temp_bed.soft_pwm_amount = 0;
      WRITE_HEATER_BED(LOW);
      LimitT=counter_ms+1000UL;
    }
  }

Включаю выключаю подогрев кровати по истечении времени. Однако период между включением и выключением всегда 5 секунд, не зависимо какой таймер я установил. Такой же таймер используется и в самом марлине. Может я его не правильно реализовал?