From e27bc0d3e0347d95feb8e43f0df490d6d8e8c4cb Mon Sep 17 00:00:00 2001
From: Maksym <maksimgrs98@gmail.com>
Date: Thu, 5 Sep 2024 03:17:57 +0200
Subject: [PATCH] Added threshold for activation point of buttons Added filters
 for smoothing results of sensors

---
 .vscode/settings.json |   3 +-
 src/main.cpp          | 105 +++++++++++++++++++++++++++++-------------
 2 files changed, 74 insertions(+), 34 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 405c118..b795e53 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -16,7 +16,8 @@
     "iomanip": "cpp",
     "limits": "cpp",
     "numeric": "cpp",
-    "streambuf": "cpp"
+    "streambuf": "cpp",
+    "random": "cpp"
   },
   "C_Cpp_Runner.cCompilerPath": "gcc",
   "C_Cpp_Runner.cppCompilerPath": "g++",
diff --git a/src/main.cpp b/src/main.cpp
index 9fef4b7..5d37e91 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -2,14 +2,22 @@
 #include <pico.h>
 #include <hardware/pwm.h>
 #include <Keyboard.h>
+
 #define PORT 2
 #define PORT2 3
+#define THRESHOLD 10
+
 class HallButton
 {
 public:
   uint64_t firstPulseStart;
-  uint64_t firstPulseLength;
-  bool isDataReadyToRead;
+  uint8_t counter = 0;
+  uint64_t firstPulseLength[3]; // value of button
+  int64_t value;
+  uint64_t _median;
+  bool isDataReadyToRead = false;
+  bool isButtonPressed = false;
+  uint64_t nextTogglePoint = 0; // value when to toggle state of button
 
   HallButton(uint8_t gpioNum, char buttonOnKeyboard)
   {
@@ -21,8 +29,10 @@ public:
   {
     if (gpioState)
     {
-      firstPulseLength = currentTime - firstPulseStart;
+      firstPulseLength[counter] = currentTime - firstPulseStart;
       isDataReadyToRead = true;
+      if (++counter >= 3)
+        counter = 0;
     }
     else
     {
@@ -31,24 +41,71 @@ public:
   }
   void HandleButton()
   {
-    if (!buttonPressed && firstPulseLength > 310)
+    if (!isDataReadyToRead)
+      return;
+    _median = median(firstPulseLength[0], firstPulseLength[1], firstPulseLength[2]);
+    value = filtered((float)_median);
+    if (nextTogglePoint == 0)
+      nextTogglePoint = value + THRESHOLD;
+    if (isButtonPressed)
     {
-      Keyboard.press(buttonOnKeyboard);
-      buttonPressed = true;
+      if (value < nextTogglePoint)
+      {
+        Release();
+      }
+      if ((int64_t)value - (int64_t)nextTogglePoint > THRESHOLD)
+        nextTogglePoint = value - THRESHOLD;
     }
-    else if (buttonPressed && firstPulseLength < 300)
+    else
     {
-      Keyboard.release(buttonOnKeyboard);
-      buttonPressed = false;
+      if (value >= nextTogglePoint)
+      {
+        Press();
+      }
+      if ((int64_t)nextTogglePoint - (int64_t)value > THRESHOLD)
+        nextTogglePoint = value + THRESHOLD;
     }
+    isDataReadyToRead = false;
+  }
+  uint64_t median(uint64_t a, uint64_t b, uint64_t c)
+  {
+    return (a < b) ? ((b < c) ? b : ((c < a) ? a : c)) : ((a < c) ? a : ((c < b) ? b : c));
+  }
+  float filtered(float value)
+  {
+    float _kalman_gain, _current_estimate;
+    _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
+    _current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
+    _err_estimate = (1.0 - _kalman_gain) * _err_estimate + fabs(_last_estimate - _current_estimate) * _q;
+    _last_estimate = _current_estimate;
+    return _current_estimate;
   }
 
 private:
-  bool buttonPressed;
+  float _err_measure = 15;
+  float _err_estimate = 15;
+  float _q = 0.1;
+  float _last_estimate = 0.0;
   uint8_t gpioNum;
   char buttonOnKeyboard;
+  void Press()
+  {
+    if (isButtonPressed)
+      return;
+    Keyboard.press(buttonOnKeyboard);
+    isButtonPressed = true;
+  }
+  void Release()
+  {
+    if (!isButtonPressed)
+      return;
+    Keyboard.release(buttonOnKeyboard);
+    isButtonPressed = false;
+  }
 };
+
 HallButton btn[2] = {HallButton(PORT, 't'), HallButton(PORT2, 'y')};
+
 void isr(uint gpio, uint32_t events)
 {
   uint64_t currentTime = time_us_64();
@@ -66,8 +123,6 @@ void isr(uint gpio, uint32_t events)
 
 void setup()
 {
-  Keyboard.begin();
-  Serial.begin(115200);
   gpio_init(PORT);
   gpio_set_dir(PORT, false);
   gpio_pull_up(PORT);
@@ -76,30 +131,14 @@ void setup()
   gpio_set_dir(PORT2, false);
   gpio_pull_up(PORT2);
   gpio_set_irq_enabled_with_callback(PORT2, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, isr);
+  // Serial.begin(9600);
+  Keyboard.begin();
 }
-#define printf Serial.printf
-byte counter = 0;
+
 void printDebugInfo()
 {
-  if (counter < 1 && btn[0].isDataReadyToRead)
-  {
-    btn[0].isDataReadyToRead = false;
-    printf("BTN1: %lu", btn[0].firstPulseLength);
-    counter++;
-  }
-  if (btn[1].isDataReadyToRead)
-  {
-    btn[1].isDataReadyToRead = false;
-    if (counter > 0)
-      printf("\t");
-    printf("BTN2: %lu", btn[1].firstPulseLength);
-    counter++;
-  }
-  if (counter > 1)
-  {
-    printf("\n");
-    counter = 0;
-  }
+  Serial.printf("%llu,%llu,%llu,%llu,%i,", btn[0].firstPulseLength[btn[0].counter], btn[0]._median, btn[0].value, btn[0].nextTogglePoint, btn[0].isButtonPressed);
+  Serial.printf("%llu,%llu,%llu,%llu,%i\n", btn[1].firstPulseLength[btn[1].counter], btn[1]._median, btn[1].value, btn[1].nextTogglePoint, btn[1].isButtonPressed);
 }
 void loop()
 {