Implement debounce utility for fs4

libraries/FilterUtils/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.19)
+cmake_policy(VERSION 3.19)
+
+set(CMAKE_CXX_STANDARD 17)
+
+project(filter_utils LANGUAGES CXX)
+
+add_definitions(
+        -DDEVICE_ANALOGIN=1
+)
+
+add_subdirectory(../../mbed-os mbed-os)
+
+add_library(filter_utils STATIC
+        debounced_digital_in.cpp
+        filtered_analog_in.cpp
+)
+
+target_include_directories(filter_utils PUBLIC include)
+
+target_link_libraries(filter_utils PUBLIC mbed-core-flags)

libraries/FilterUtils/README.md

@@ -0,0 +1,184 @@
+# SensorFilters
+
+This library provides classes that filter inputs to increase reliability and robustness.
+
+## Classes
+
+___
+
+### DebouncedDigitalIn
+
+Debounces a digital input.
+
+#### Implementation
+Digital inputs are debounced by using a class-wide timer to sample all the input at a high frequency (1ms), and only updates the debounced state of each pin when the signal has changed for a set number of samples (_valid_read_count).
+
+The signal must change for a time equal to the sampling interval multiplied by the required sample count, for example, with default values, 5 samples are required, so a signal must change for 5ms straight to be registered. If the signal is read to be the same as the current default state, then the counted number of changed reads is reset back to 0.
+
+If you pass in values <= 1 as the valid_read_count, then the DebouncedDigitalIn will act largely the same as a normal DigitalIn, just updating every ms rather than on every read. If, for example, you set valid_read_count to 3, then the DebouncedDigitalIn object will need to read a changed value 3 times in a row for the returned default state to then change.
+
+The default state for a DebouncedDigitalIn object is false (0).
+
+#### Code Sample
+```cpp
+#include "mbed.h"
+#include "DebouncedDigitalIn.h"
+
+//  Initializes a normal DigitalIn to pin PA_6.
+DigitalIn button(PA_6);
+
+//  Passes in a reference to the DigitalIn and sets the valid read count to 5.
+//  The valid read count is simply the amount of different reads required in
+//  a row for the debounced_in to return the different value. With our approach,
+//  each DebouncedDigitalIn is rechecked every ms at the same time. This means
+//  that the valid read count is the amount of ms that the signal needs to
+//  change for a different value to be returned (HIGH -> LOW or LOW -> HIGH).
+//  (See: https://my.eng.utah.edu/~cs5780/debouncing.pdf, Sections: Software
+//  Debouncers-A Counting Algorithm)
+DebouncedDigitalIn debounced_button(button, 5);
+
+int main() {
+    //  Sets the DebouncedDigitalIn object's valid read count to 7 (there must
+    //  be a changed state for 7ms for the returned value of read() to change)
+    debounced_button.set_valid_read_count(7);
+
+    while (true) {
+        //  Runs the default read() function on the DigitalIn.
+        const bool default_button_state = button.read();
+
+        //  Returns what the current debounced state is. Keep in mind that the
+        //  read() does NOT run an update to the current debounced state since
+        //  that happens at the same time for all objects every ms.
+        const bool debounced_button_state = debounced_button.read();
+    }
+
+    return 0;
+}
+```
+
+___
+
+### FilteredAnalogIn
+
+Smooths an analog signal like a first-pass RC filter.
+
+#### Implementation
+The FilteredAnalogIn class implements an Exponential Weighted Moving Average (EWMA) filter to smooth out erratic analog inputs by applying a kind of digital low pass filter. 
+
+The EWMA filter works according to the following formula:
+<br />
+<br />
+<img width="932" height="106" alt="ewma-formula" src="https://github.com/user-attachments/assets/25deed81-95f4-4969-9351-fc398b5b2128" />
+<br />
+<br />
+Where:
+- Vsmoothed​(t) is the new smoothed value (_smoothed_value).
+- Vraw​(t) is the current raw reading (raw_value from _analog_pin.read()).
+- Vsmoothed​(t−Δt) is the previous smoothed value.
+- $\alpha$ is the smoothing factor (or weight).
+
+$\alpha$, the smoothing factor is based on the time constant (_time_constant) and time difference between readings. $\alpha$ is derived as follows:
+<br />
+<br />
+<img width="264" height="98" alt="alpha-formula" src="https://github.com/user-attachments/assets/6711ed83-d87f-42c9-95c2-972c077f9b4d" />
+<br />
+<br />
+This way, the cutoff frequency, which is used in determining the time constant as per the equation below, stays constant even if the time between reads is not consistent. Using the cutoff frequency in this way allows the digital filtering to behave the same as a first-pass RC filter would.
+<br />
+<br />
+<img width="250" height="148" alt="time-constant-formula" src="https://github.com/user-attachments/assets/1460fb1d-6d14-4551-b960-6459aa9365ac" />
+<br />
+<br />
+The time constant 𝜏 is the time required for the filter's output to reach ≈ 70.7% of the final value after a sudden step change. A larger time constant results in more smoothing but a slower response, while a smaller time constant results in less smoothing but a faster response.
+
+Here's a graph of a test run which shows how effective this method is at filtering noisy analog sensor input:
+<br />
+<br />
+![ewma-graph-resize](https://github.com/user-attachments/assets/ff50976d-4ede-44f0-9e32-5e186e9443f4)
+<br />
+
+#### Other Methods
+Two other options for smoothing are Rolling Average and Exponential Moving Average. 
+<br />
+<br />
+<img width="550" height="300" alt="RA vs EMA vs EWMA" src="https://github.com/user-attachments/assets/5ce7f78b-eba7-496a-badc-cb864d6f9f4e" />
+- Rolling Average: Yellow
+    - Treats all data points within a fixed window equally.
+    - Causes a lag where smoothed output is slow to react to a sudden change in sensor reading (step change).
+    - Requires allocated memory to properly make windows.
+
+
+- Exponential Moving Average: Blue
+    - Uses an exponentially decreasing weighting factor ($\alpha$) for older observations. Recent data points contribute more significantly to the average than data points further in the past.
+    - Reduces lag compared to the Rolling Average because it reacts faster to recent changes.
+    - Sampling time variance can cause issues since all reads are treated the same.
+
+
+- Exponential Weighted Moving Average: Purple
+  - The approach that the FilteredAnalogIn objects use.
+  - An exponential decaying weight is implemented to past data points. Same fundamental weighting scheme as the EMA.
+  - Uses the same filtering as a First-Order Low-Pass Filter, where the -3db level is at the cutoff frequency.
+      -  Low-Pass Filter: Allows low-frequency signals (the underlying trend) to pass through, while attenuating high-frequency signals (noise or rapid fluctuations).
+      -  Keep in mind that the -3db level is where ~70.7% of the signals at that frequency pass through, following a decreasing pattern past that point.
+##### Code Sample 1
+```cpp
+#include "mbed.h"
+#include "FilteredAnalogIn.h"
+
+//  Initializes a normal AnalogIn to pin PA_6.
+AnalogIn in(PA_6);
+
+//  Passes in a reference to the AnalogIn and sets the cutoff_frequency to 10Hz.
+//  The cutoff frequency is similar to a first-pass RC filter, where the frequency
+//  given is put at the -3db level.
+//  (See: https://www.electronics-tutorials.ws/filter/filter_2.html, Sections: RC
+//  Time Constant & Frequency Response of a 1st-order Low Pass Filter)
+FilteredAnalogIn smoothed_in(in, 10);
+
+int main() {
+    //  Sets the FilteredAnalogIn object's cutoff frequency to 100Hz.
+    smoothed_in.set_time_constant(100);
+    //  Sets the referenced AnalogIn ("in" for this case) to have a
+    //  reference_voltage of 3.3V.
+    smoothed_in.set_reference_voltage(3.3);
+
+    while (true) {
+        //  Runs the default read_voltage() function on the AnalogIn.
+        const float default_voltage = in.read_voltage();
+
+        //  Runs the EWMA-smoothing algorithm on the current read value and returns
+        //  the smoothed read as a result.
+        const float smoothed_voltage = smoothed_in.read_voltage();
+    }
+
+    return 0;
+}
+```
+
+##### Code Sample 2
+```cpp
+#include "mbed.h"
+#include "FilteredAnalogIn.h"
+
+//  Initializes a normal AnalogIn to pin PA_7.
+AnalogIn temp_sensor(PA_7);
+
+FilteredAnalogIn smoothed_temp(temp_sensor, 2);
+
+int main(){
+    // Set time constant to 0.1 secounds (𝜏 = 0.1s)
+    // Also equivalent to a cutoff frequency of fc = 1/(2*π*0.1) = 1.59 Hz.
+    smoothed_temp.set_time_constant(0.1);
+
+    while(true){
+        // Read current smoothed value from 0.0-1.0 range.
+        const float smoothed_value = smoothed_temp.read();
+        // Smoothed value used for calculations and logging.
+        // To calculate temp from 0-1 range:
+        // const float current_temp = smoothed_value * 100.0f;
+    }
+    return 0;
+}
+```
+___
+

libraries/FilterUtils/debounced_digital_in.cpp

@@ -0,0 +1,31 @@
+//
+// Created by Jackson Pinsonneault on 10/27/25.
+//
+
+#include "debounced_digital_in.h"
+
+int DebouncedDigitalIn::read() const {
+    return _current_state;
+}
+
+int DebouncedDigitalIn::is_connected() const {
+    return _digital_pin.is_connected();
+}
+
+void DebouncedDigitalIn::set_valid_read_count(const uint16_t valid_read_count) {
+    _valid_read_count = valid_read_count;
+}
+
+void DebouncedDigitalIn::add_sample() {
+    if (_digital_pin.read() == _current_state) {
+        _changed_state_time = 0;
+        return;
+    }
+
+    _changed_state_time++;
+
+    if (_changed_state_time >= _valid_read_count) {
+        _current_state = !_current_state;
+        _changed_state_time = 0;
+    }
+}

libraries/FilterUtils/filtered_analog_in.cpp

@@ -0,0 +1,43 @@
+//
+// Created by Jackson Pinsonneault on 10/28/25.
+//
+
+#include "filtered_analog_in.h"
+#include <numbers>
+
+float FilteredAnalogIn::read() {
+    const unsigned long time_dif = chrono::duration_cast<std::chrono::microseconds>(_timer.elapsed_time()).count();
+    const float raw_value = _analog_pin.read();
+    _timer.reset();
+
+    if (!_is_initialized) {
+        _smoothed_value = raw_value;
+        _is_initialized = true;
+    } else {
+        const float exponent = -1.0 * static_cast<float>(time_dif) / pow(10,6) / _time_constant;
+        const float exponential_component = exp(exponent);
+        _smoothed_value = (1 - exponential_component) * raw_value + exponential_component * _smoothed_value;
+    }
+
+    return _smoothed_value;
+}
+
+unsigned short FilteredAnalogIn::read_u16() {
+    return static_cast<unsigned short>(0xFFFF * read());
+}
+
+float FilteredAnalogIn::read_voltage() {
+    return read() * _analog_pin.get_reference_voltage();
+}
+
+void FilteredAnalogIn::set_reference_voltage(float vref) const {
+    _analog_pin.set_reference_voltage(vref);
+}
+
+float FilteredAnalogIn::get_reference_voltage() const {
+    return _analog_pin.get_reference_voltage();
+}
+
+void FilteredAnalogIn::set_time_constant(const float cutoff_frequency) {
+    _time_constant = 1.0 / (2.0 * M_PI * cutoff_frequency);
+}

libraries/FilterUtils/include/debounced_digital_in.h

@@ -0,0 +1,72 @@
+//
+// Created by Jackson Pinsonneault on 10/27/25.
+//
+
+#ifndef LIBRARIES_DEBOUNCE_H
+#define LIBRARIES_DEBOUNCE_H
+
+#include <cstdint>
+#include "mbed.h"
+
+class DebouncedDigitalIn;
+inline void sample_all_digital_pins();
+
+inline Ticker debounce_ticker;
+inline bool ticker_started = false;
+inline std::vector<DebouncedDigitalIn*> digital_pins;
+
+class DebouncedDigitalIn {
+
+public:
+    /** Create an auto-debouncing DigitalIn
+     *
+     * @param digital_pin Reference to a DigitalIn to use for reading
+     * @param valid_read_count (optional) Amount of consecutive reads of the same value needed to change what read() returns (defaults to 5)
+    */
+    explicit DebouncedDigitalIn(DigitalIn &digital_pin, const uint16_t valid_read_count=5) :
+        _digital_pin(digital_pin),
+        _valid_read_count(valid_read_count) {
+        digital_pins.push_back(this);
+
+        if (!ticker_started) {
+            debounce_ticker.attach(&sample_all_digital_pins, 1ms);
+            ticker_started = true;
+        }
+    }
+
+    /** Read the debounced referenced DigitalIn input, represented as 0 or 1 (int)
+     *
+     * @returns An integer representing the debounced state of the referenced DigitalIn input, 0 for logical 0, 1 for logical 1
+    */
+    int read() const;
+
+    /** Return the referenced DigitalIn's output setting, represented as 0 or 1 (int)
+     *
+     * @returns Non-zero value if referenced pin is connected to uc GPIO, 0 if referenced gpio object was initialized with NC
+    */
+    int is_connected() const;
+
+    /** Sets how resistant the debounced state is to changing
+     *
+     * @param valid_read_count Amount of consecutive reads of the same value needed to change what read() returns
+    */
+    void set_valid_read_count(uint16_t valid_read_count);
+
+    /** Updates the current debounced state of the referenced DigitalIn
+    */
+    void add_sample();
+
+private:
+    DigitalIn &_digital_pin;            //  DigitalIn which is used for the reads
+    bool _current_state = false;        //  current debounced state
+    uint16_t _valid_read_count;         //  amount of consecutive reads of the same value to change current_state
+    uint16_t _changed_state_time = 0;   //  how many consecutive reads (that aren't the same as current_state) have occurred
+};
+
+inline void sample_all_digital_pins() {
+    for (DebouncedDigitalIn *debounced_digital : digital_pins) {
+        debounced_digital->add_sample();
+    }
+}
+
+#endif //LIBRARIES_DEBOUNCE_H