WaterTank/SensorMonitor.cpp at main · fredy314/WaterTank · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
/*
 * WaterTank project
 * Copyright (c) 2026 Fedir Vilhota <fredy31415@gmail.com>
 * This software is released under the MIT License.
 * See the LICENSE file in the project root for full license information.
 */

// SensorMonitor.cpp
#include "SensorMonitor.h" // Включаємо свій же заголовок
#include "Constants.h"
#include <algorithm>

SensorMonitor::SensorMonitor()
    // Ініціалізація констант-членів класу за допомогою списку ініціалізації
    : _PIN_TRIG(PIN_TRIG),
      _PIN_ECHO(PIN_ECHO),
      _LEVEL_MIN(LEVEL_MIN),
      _LEVEL_MAX(LEVEL_MAX)
{
  pinMode(_PIN_TRIG, OUTPUT);
  pinMode(_PIN_ECHO, INPUT);
  digitalWrite(_PIN_TRIG, HIGH);
  for (int i = 0; i < _ROLLING_DISTANCE_WINDOW_SIZE; i++) {
    _distanceReadingsForCurrent[i] = 0.0;
  }
}


float SensorMonitor::getDistance() {
    // Видаємо короткий LOW імпульс на TRIG, щоб переконатися, що він "чистий"
  // Перевірка, чи вільний пін Echo
  if (digitalRead(_PIN_ECHO) == HIGH && DEBUG) {
    Serial.println("Sensor Warning: Echo pin is HIGH before Trig (Sensor busy/noisy?)!");
  }
    // Видаємо короткий LOW імпульс на TRIG, щоб переконатися, що він "чистий"
  digitalWrite(_PIN_TRIG, HIGH);
  delayMicroseconds(2);
  // Видаємо HIGH імпульс на TRIG протягом 10 мікросекунд, щоб запустити датчик
  digitalWrite(_PIN_TRIG, LOW);
  delayMicroseconds(10);
  digitalWrite(_PIN_TRIG, HIGH);
  // Вимірюємо тривалість HIGH імпульсу на ECHO-піні (час подорожі звуку)
  unsigned long timeout = 30000; // 30ms timeout (~5m max distance)
  long duration = pulseIn(_PIN_ECHO, HIGH, timeout);
  // Розраховуємо відстань:
  // Швидкість звуку в повітрі приблизно 343 метри/секунду, або 0.0343 см/мікросекунду.
  // Імпульс проходить до об'єкта і повертається, тому ділимо на 2.
  float rawDistance = duration * 0.0343 / 2;

  if (DEBUG) {
    if (duration == 0) {
      Serial.println("Sensor Error: Pulse Timeout (Check wiring/voltage divider/5V power)");
    } else if (duration < 100) { // Менше ~1.7см - скоріше за все завада
      Serial.print("Sensor Warning: Noise/Spike detected (Duration: ");
      Serial.print(duration);
      Serial.println(" us)");
    } else {
      Serial.print("Raw Distance: ");
      Serial.print(rawDistance);
      Serial.print(" cm (Duration: ");
      Serial.print(duration);
      Serial.println(" us)");
    }
  }

  // Обробка помилкових значень (не оновлюємо медіану занадто малими або нульовими даними)
  if (duration < 100 || duration > 30000) {
    // Повертаємо останнє відоме значення або медіану, не оновлюючи буфер
    return _distanceReadingsForCurrent[(_rollingDistanceIndex - 1 + _ROLLING_DISTANCE_WINDOW_SIZE) % _ROLLING_DISTANCE_WINDOW_SIZE];
  }

  _distanceReadingsForCurrent[_rollingDistanceIndex] = rawDistance;
  _rollingDistanceIndex = (_rollingDistanceIndex + 1) % _ROLLING_DISTANCE_WINDOW_SIZE;
  if(_rollingDistanceCount<_ROLLING_DISTANCE_WINDOW_SIZE) {
    _rollingDistanceCount++;
    return rawDistance;
  } else {
    float tempReadings[_ROLLING_DISTANCE_WINDOW_SIZE];
    memcpy(tempReadings, _distanceReadingsForCurrent, sizeof(float) * _ROLLING_DISTANCE_WINDOW_SIZE);
    std::sort(tempReadings, tempReadings + _ROLLING_DISTANCE_WINDOW_SIZE);
    return tempReadings[_ROLLING_DISTANCE_WINDOW_CENTER];
  }
}