Нужна помощь в отладке кода частоты сердечных сокращений/фильтра (вычисление частоты сердечных сокращений не работает)

Приведенный ниже код представляет собой программу, предназначенную для применения алгоритма Панса-Томпскина к сигналу, полученному от фильтра ЭКГ. Фильтр, кажется, работает нормально, но он не может мигать светодиодом и правильно измерять частоту сердечных сокращений. Порог может быть проблемой, но я изо всех сил пытаюсь понять, что именно правильно.

    #include <bluefruit.h>
    
    int Threshold =160000;  // Измерение пульса.
    #define Threshold2 600 // Время импульса.
    #define INTERVAL 10 // Используется для 25-миллисекундного таймера.
    
    //инициация для частоты сердечных сокращений
    unsigned long Timer;
    unsigned long Timer2;// Объявляет два таймера.
    
    enum Heart { Wait, Idle, Update};
    enum Heart HeartState;// Используется для состояний в конечном автомате ниже.
    
    float HBI;
    float SixHBI = 0.0;
    float HBIAverage = 0.0;
    float BPM = 0.0;
    int Count = 0;
    float filtered = 0;
    
    
    //инициализация фильтра dg
    
    float xn1 = 0; //объявить xn1 как ноль
    float yn1 = 0;
    int k = 0;
    
    //скользящее среднее
    const int numReadings  = 30;
    int readings [numReadings];
    int readIndex  = 0;
    long total  = 0;
    long average;
    
    void HbiStd(float Value) {
      switch (HeartState) {
        case Wait:
          if (millis() - Timer2 > Threshold2) {
            HeartState = Idle;
            // Если прошло 600 миллисекунд, переходим в состояние ожидания.
          }
          break;
        case Idle:
          if (Value > Threshold) {
            HeartState = Update;
            // Если обнаружено сердцебиение, перейдите в состояние обновления.
          }
          break;
          
        case Update:
          HBI = millis() - Timer2; // Сохраняет время HBI.
          Timer2 += HBI; // Увеличивает Timer2.
          SixHBI += HBI; // Складываем все HBI вместе.
          HBIAverage = SixHBI / 6000; // Вычисляет среднее значение HBI.
          BPM = (1 / HBIAverage) * (60); // Вычисляет BPM.
          Count++; // Увеличивает проход, на котором он находится.
          if (Count == 6) { // Конечный автомат проходит 6 раз.
    
           // Serial.print(String(BPM) + String("BPM\n"));
            // Печатает BPM.
            SixHBI = 0;
            Count = 0;
            // Сбрасывает HBI и счетчик.
          }
          HeartState = Wait; // Вернуться в состояние ожидания.
          break;
      }
    }
    
    
    
    // поместите сюда код установки для однократного запуска:
    void setup() {
      // инициализируем цифровой вывод LED_BUILTIN как выход.
      pinMode(LED_BUILTIN, OUTPUT);
    
      analogReadResolution(12);
      Serial.begin(115200);
    
      Timer = millis();      // Настройка таймера.
      Timer2 = millis(); // Запустить Таймер2.
      HeartState = Wait; // Конечный автомат запускается в состоянии ожидания.
    }
    
    
    uint8_t INPUT_PIN = A1;
    
    void loop() {
      //переменная для фильтра скользящего среднего
    
    
      if (millis() - Timer >= INTERVAL)
      { float raw = analogRead(INPUT_PIN);//чтение данных из ввода;
        filtered = Pan_thom_alg(raw);
      
    
        if (k % 3 == 0)
        {
          //int filtered = raw*raw;//квадрат
          // Serial.print(String(raw) + String("raw\n"));
             Serial.print(String(filtered) + String(" filtered\n"));
     
    
          if (filtered > Threshold) {
            digitalWrite(LED_BUILTIN, HIGH);
            delay(50);
          }
          if (filtered < Threshold)  {
            digitalWrite(LED_BUILTIN, HIGH);
            // задержка (10);
          }
        }
        k = k + 1;
    
    
        Timer += INTERVAL;
      }
    HbiStd(filtered); // Конечный автомат вызывается и всегда работает.
      // задержка (10);
    
    
    
    
      //печать после того, как 3 записи записаны для метки
    
    
    }
    
    
    float Pan_thom_alg(float raw) {
      float xn = raw;
      filtered = -0.00084 * yn1 + 0.999 * xn + -0.0008 * xn1;
      xn1 = xn;
      yn1 = filtered;
    
      // квадратные данные
      filtered = sq(filtered);
    
      // вычесть последнее чтение:
      total = total - readings[readIndex];
      // читаем датчик:
      readings[readIndex] = filtered;
      // добавить значение к итогу:
      total = total + readings[readIndex];
      // обрабатывать индекс
      readIndex = readIndex + 1;
      if (readIndex >= numReadings) {
        readIndex = 0;
      }
      // вычисляем среднее:
      average = total / numReadings;
      filtered = average;
      return filtered;
    }