Add multi tap boolean stream filter and multi tap trigger modifier (double tap detector)

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java

@@ -7,6 +7,7 @@
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.MultiTapFilter;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -287,4 +288,31 @@ public boolean getAsBoolean() {
           }
         });
   }
+
+  /**
+   * Creates a new multi-tap trigger from this trigger - it will become active when this trigger has
+   * been activated the required number of times within the specified time window.
+   *
+   * <p>This is useful for implementing "double-click" style functionality.
+   *
+   * <p>Input for this must be stable, consider using a Debouncer before this filter to avoid
+   * counting noise as multiple taps.
+   *
+   * @param requiredTaps The number of taps required.
+   * @param tapWindowSeconds The time window in which the taps must occur.
+   * @return The multi-tap filtered trigger.
+   */
+  public Trigger multiTap(int requiredTaps, double tapWindowSeconds) {
+    return new Trigger(
+        m_loop,
+        new BooleanSupplier() {
+          final MultiTapFilter m_multiTapFilter =
+              new MultiTapFilter(requiredTaps, tapWindowSeconds);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_multiTapFilter.calculate(m_condition.getAsBoolean());
+          }
+        });
+  }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -7,6 +7,7 @@
 #include <utility>
 
 #include <frc/filter/Debouncer.h>
+#include <frc/filter/MultiTapFilter.h>
 
 #include "frc2/command/CommandPtr.h"
 
@@ -184,6 +185,13 @@ Trigger Trigger::Debounce(units::second_t debounceTime,
   });
 }
 
+Trigger Trigger::MultiTap(int requiredTaps, units::second_t tapWindow) {
+  return Trigger(m_loop, [filter = frc::MultiTapFilter(requiredTaps, tapWindow),
+                          condition = m_condition]() mutable {
+    return filter.Calculate(condition());
+  });
+}
+
 bool Trigger::Get() const {
   return m_condition();
 }

wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h

@@ -284,6 +284,22 @@ class Trigger {
                    frc::Debouncer::DebounceType type =
                        frc::Debouncer::DebounceType::kRising);
 
+  /**
+   * Creates a new multi-tap trigger from this trigger - it will become active
+   * when this trigger has been activated the required number of times within
+   * the specified time window.
+   *
+   * This is useful for implementing "double-click" style functionality.
+   *
+   * Input for this must be stable, consider using a Debouncer before this
+   * filter to avoid counting noise as multiple taps.
+   *
+   * @param requiredTaps The number of taps required.
+   * @param tapWindowSeconds The time window in which the taps must occur.
+   * @return The multi-tap filtered trigger.
+   */
+  Trigger MultiTap(int numTaps, units::second_t interval);
+
   /**
    * Returns the current state of this trigger.
    *

wpimath/src/main/java/edu/wpi/first/math/filter/MultiTapFilter.java

@@ -0,0 +1,117 @@
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+
+/**
+ * A simple tap counting filter for boolean streams. Requires that the boolean change value to true
+ * for a specified number of times within a specified time window after the first rising edge before
+ * the filtered value changes.
+ *
+ * <p>The filter activates when the input has risen (transitioned from false to true) the required
+ * number of times within the time window. Once activated, the output remains true as long as the
+ * input is true. The tap count resets when the time window expires or when the input goes false
+ * after activation.
+ *
+ * <p>Input must be stable; consider using a Debouncer before this filter to avoid counting noise as
+ * multiple taps.
+ */
+public class MultiTapFilter {
+
+  private int m_requiredTaps;
+  private double m_tapWindowSeconds;
+
+  private double m_firstTapTimeSeconds;
+  private int m_currentTapCount;
+
+  private boolean m_lastInput;
+
+  /**
+   * Creates a new MultiTapFilter.
+   *
+   * @param requiredTaps The number of rising edges (taps) required before the output goes true.
+   * @param tapWindowSeconds The maximum time window in which all required taps must occur after the
+   *     first rising edge.
+   */
+  public MultiTapFilter(int requiredTaps, double tapWindowSeconds) {
+    m_requiredTaps = requiredTaps;
+    m_tapWindowSeconds = tapWindowSeconds;
+
+    resetTimer();
+  }
+
+  private void resetTimer() {
+    m_firstTapTimeSeconds = MathSharedStore.getTimestamp();
+  }
+
+  private boolean hasElapsed() {
+    return MathSharedStore.getTimestamp() - m_firstTapTimeSeconds >= m_tapWindowSeconds;
+  }
+
+  /**
+   * Applies the tap counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of taps have occurred within the time window and the input
+   *     is currently true; false otherwise.
+   */
+  public boolean calculate(boolean input) {
+    boolean enoughTaps = m_currentTapCount >= m_requiredTaps;
+
+    boolean expired = hasElapsed() && !enoughTaps;
+    boolean activationEnded = !input && enoughTaps;
+
+    if (expired || activationEnded) {
+      m_currentTapCount = 0;
+    }
+
+    if (input && !m_lastInput) {
+      if (m_currentTapCount == 0) {
+        resetTimer(); // Start timer on first rising edge
+      }
+
+      m_currentTapCount++;
+    }
+
+    m_lastInput = input;
+
+    return input && m_currentTapCount >= m_requiredTaps;
+  }
+
+  /**
+   * Sets the tap window duration.
+   *
+   * @param tapWindowSeconds The maximum time window in which all required taps must occur after the
+   *     first rising edge.
+   */
+  public void setTapWindowSeconds(double tapWindowSeconds) {
+    m_tapWindowSeconds = tapWindowSeconds;
+  }
+
+  /**
+   * Gets the tap window duration.
+   *
+   * @return The maximum time window in which all required taps must occur after the first rising
+   *     edge.
+   */
+  public double getTapWindowSeconds() {
+    return m_tapWindowSeconds;
+  }
+
+  /**
+   * Sets the required number of taps.
+   *
+   * @param requiredTaps The number of rising edges (taps) required before the output goes true.
+   */
+  public void setRequiredTaps(int requiredTaps) {
+    m_requiredTaps = requiredTaps;
+  }
+
+  /**
+   * Gets the required number of taps.
+   *
+   * @return The number of rising edges (taps) required before the output goes true.
+   */
+  public int getRequiredTaps() {
+    return m_requiredTaps;
+  }
+}

wpimath/src/main/native/cpp/filter/MultiTapFilter.cpp

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/filter/MultiTapFilter.h"
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+MultiTapFilter::MultiTapFilter(int requiredTaps, units::second_t tapWindow)
+    : m_requiredTaps(requiredTaps), m_tapWindow(tapWindow) {
+  ResetTimer();
+}
+
+void MultiTapFilter::ResetTimer() {
+  m_firstTapTime = wpi::math::MathSharedStore::GetTimestamp();
+}
+
+bool MultiTapFilter::HasElapsed() const {
+  return wpi::math::MathSharedStore::GetTimestamp() - m_firstTapTime >=
+         m_tapWindow;
+}
+
+bool MultiTapFilter::Calculate(bool input) {
+  bool enoughTaps = m_currentTapCount >= m_requiredTaps;
+
+  bool expired = HasElapsed() && !enoughTaps;
+  bool activationEnded = !input && enoughTaps;
+
+  if (expired || activationEnded) {
+    m_currentTapCount = 0;
+  }
+
+  if (input && !m_lastInput) {
+    if (m_currentTapCount == 0) {
+      ResetTimer();  // Start timer on first rising edge
+    }
+
+    m_currentTapCount++;
+  }
+
+  m_lastInput = input;
+
+  return input && m_currentTapCount >= m_requiredTaps;
+}

wpimath/src/main/native/include/frc/filter/MultiTapFilter.h

@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "units/time.h"
+
+namespace frc {
+/**
+ * A simple tap counting filter for boolean streams. Requires that the boolean
+ * change value to true for a specified number of times within a specified time
+ * window after the first rising edge before the filtered value changes.
+ *
+ * The filter activates when the input has risen (transitioned from false to
+ * true) the required number of times within the time window. Once activated,
+ * the output remains true as long as the input is true. The tap count resets
+ * when the time window expires or when the input goes false after activation.
+ *
+ * Input must be stable; consider using a Debouncer before this filter to
+ * avoid counting noise as multiple taps.
+ */
+class WPILIB_DLLEXPORT MultiTapFilter {
+ public:
+  /**
+   * Creates a new MultiTapFilter.
+   *
+   * @param requiredTaps The number of rising edges (taps) required before the
+   *                     output goes true.
+   * @param tapWindow The maximum time window in which all required taps must
+   *                  occur after the first rising edge.
+   */
+  explicit MultiTapFilter(int requiredTaps, units::second_t tapWindow);
+
+  /**
+   * Applies the tap counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of taps have occurred within the time
+   *         window and the input is currently true; false otherwise.
+   */
+  bool Calculate(bool input);
+
+  /**
+   * Sets the tap window duration.
+   *
+   * @param tapWindow The maximum time window in which all required taps must
+   *                  occur after the first rising edge.
+   */
+  constexpr void SetTapWindow(units::second_t tapWindow) {
+    m_tapWindow = tapWindow;
+  }
+
+  /**
+   * Gets the tap window duration.
+   *
+   * @return The maximum time window in which all required taps must occur
+   *         after the first rising edge.
+   */
+  constexpr units::second_t GetTapWindow() const { return m_tapWindow; }
+
+  /**
+   * Sets the required number of taps.
+   *
+   * @param requiredTaps The number of rising edges (taps) required before the
+   *                     output goes true.
+   */
+  constexpr void SetRequiredTaps(int requiredTaps) {
+    m_requiredTaps = requiredTaps;
+  }
+
+  /**
+   * Gets the required number of taps.
+   *
+   * @return The number of rising edges (taps) required before the output goes
+   *         true.
+   */
+  constexpr int GetRequiredTaps() const { return m_requiredTaps; }
+
+ private:
+  int m_requiredTaps;
+  units::second_t m_tapWindow;
+
+  units::second_t m_firstTapTime;
+  int m_currentTapCount = 0;
+
+  bool m_lastInput = false;
+
+  void ResetTimer();
+
+  bool HasElapsed() const;
+};
+}  // namespace frc