[wpimath,cmd] Add multi tap boolean stream filter and multi tap trigger modifier (double tap detector) (#8307)

Add a simple tap counting filter for boolean streams. 

The filter activates when the input has risen (transitioned from false
to true, like when a button is tapped) the required number of times
within the time window after the first rising edge. 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.

Example usage:
```java
    xbox.a()
      .multiPress(2, 0.2) // Detect a double tap within 0.2 seconds
      .onTrue(Commands.print("Double tapped A button"));
      
     xbox.y()
      .multiPress(2, 0.5) // Detect a double tap within 0.5 seconds
      .whileTrue(Commands.print("Y held after tap").repeatedly());
```

This is not a noise reduction and/or input smoothing filter, but it is
similar in usage to debounce, so I believe it could be considered a
filter, but am open to a better location.

I believe this would be a useful addition, as double/triple tapping a
button is a common control option in games, yet is not often utilized by
newer FRC teams. I believe adding it to WPILib in a standard way will
allow more teams to make the most out of their controls.

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

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command.button;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.EdgeCounterFilter;
 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 class Trigger implements BooleanSupplier {
           }
         });
   }
+
+  /**
+   * Creates a new multi-press 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 to avoid counting
+   * noise as multiple presses.
+   *
+   * @param requiredPresses The number of presses required.
+   * @param windowTime The number of seconds in which the presses must occur.
+   * @return The multi-press trigger.
+   */
+  public Trigger multiPress(int requiredPresses, double windowTime) {
+    return new Trigger(
+        m_loop,
+        new BooleanSupplier() {
+          final EdgeCounterFilter m_edgeCounterFilter =
+              new EdgeCounterFilter(requiredPresses, windowTime);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_edgeCounterFilter.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/EdgeCountFilter.h>
 
 #include "frc2/command/CommandPtr.h"
 
@@ -184,6 +185,14 @@ Trigger Trigger::Debounce(units::second_t debounceTime,
   });
 }
 
+Trigger Trigger::MultiPress(int requiredPresses, units::second_t windowTime) {
+  return Trigger(m_loop,
+                 [filter = frc::EdgeCounterFilter(requiredPresses, windowTime),
+                  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-press 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 to
+   * avoid counting noise as multiple presses.
+   *
+   * @param requiredPresses The number of presses required.
+   * @param windowTime The time in which the presses must occur.
+   * @return The multi-press trigger.
+   */
+  Trigger MultiPress(int requiredPresses, units::second_t windowTime);
+
   /**
    * Returns the current state of this trigger.
    *

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

@@ -0,0 +1,120 @@
+// 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.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+
+/**
+ * A rising edge counter 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 edge 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 edges.
+ */
+public class EdgeCounterFilter {
+  private int m_requiredEdges;
+  private double m_windowTimeSeconds;
+
+  private double m_firstEdgeTimeSeconds;
+  private int m_currentCount;
+
+  private boolean m_lastInput;
+
+  /**
+   * Creates a new EdgeCounterFilter.
+   *
+   * @param requiredEdges The number of rising edges required before the output goes true.
+   * @param windowTime The maximum number of seconds in which all required edges must occur after
+   *     the first rising edge.
+   */
+  public EdgeCounterFilter(int requiredEdges, double windowTime) {
+    m_requiredEdges = requiredEdges;
+    m_windowTimeSeconds = windowTime;
+
+    resetTimer();
+  }
+
+  private void resetTimer() {
+    m_firstEdgeTimeSeconds = MathSharedStore.getTimestamp();
+  }
+
+  private boolean hasElapsed() {
+    return MathSharedStore.getTimestamp() - m_firstEdgeTimeSeconds >= m_windowTimeSeconds;
+  }
+
+  /**
+   * Applies the edge counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of edges have occurred within the time window and the input
+   *     is currently true; false otherwise.
+   */
+  public boolean calculate(boolean input) {
+    boolean enoughEdges = m_currentCount >= m_requiredEdges;
+
+    boolean expired = hasElapsed() && !enoughEdges;
+    boolean activationEnded = !input && enoughEdges;
+
+    if (expired || activationEnded) {
+      m_currentCount = 0;
+    }
+
+    if (input && !m_lastInput) {
+      if (m_currentCount == 0) {
+        resetTimer(); // Start timer on first rising edge
+      }
+
+      m_currentCount++;
+    }
+
+    m_lastInput = input;
+
+    return input && m_currentCount >= m_requiredEdges;
+  }
+
+  /**
+   * Sets the time window duration.
+   *
+   * @param windowTime The maximum window of seconds in which all required edges must occur after
+   *     the first rising edge.
+   */
+  public void setWindowTime(double windowTime) {
+    m_windowTimeSeconds = windowTime;
+  }
+
+  /**
+   * Gets the time window duration.
+   *
+   * @return The maximum window of seconds in which all required edges must occur after the first
+   *     rising edge.
+   */
+  public double getWindowTime() {
+    return m_windowTimeSeconds;
+  }
+
+  /**
+   * Sets the required number of edges.
+   *
+   * @param requiredEdges The number of rising edges required before the output goes true.
+   */
+  public void setRequiredEdges(int requiredEdges) {
+    m_requiredEdges = requiredEdges;
+  }
+
+  /**
+   * Gets the required number of edges.
+   *
+   * @return The number of rising edges required before the output goes true.
+   */
+  public int getRequiredEdges() {
+    return m_requiredEdges;
+  }
+}

wpimath/src/main/native/cpp/filter/EdgeCountFilter.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/EdgeCountFilter.h"
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+EdgeCounterFilter::EdgeCounterFilter(int requiredEdges, units::second_t window)
+    : m_requiredEdges(requiredEdges), m_windowTime(window) {
+  ResetTimer();
+}
+
+void EdgeCounterFilter::ResetTimer() {
+  m_firstEdgeTime = wpi::math::MathSharedStore::GetTimestamp();
+}
+
+bool EdgeCounterFilter::HasElapsed() const {
+  return wpi::math::MathSharedStore::GetTimestamp() - m_firstEdgeTime >=
+         m_windowTime;
+}
+
+bool EdgeCounterFilter::Calculate(bool input) {
+  bool enoughEdges = m_currentCount >= m_requiredEdges;
+
+  bool expired = HasElapsed() && !enoughEdges;
+  bool activationEnded = !input && enoughEdges;
+
+  if (expired || activationEnded) {
+    m_currentCount = 0;
+  }
+
+  if (input && !m_lastInput) {
+    if (m_currentCount == 0) {
+      ResetTimer();  // Start timer on first rising edge
+    }
+
+    m_currentCount++;
+  }
+
+  m_lastInput = input;
+
+  return input && m_currentCount >= m_requiredEdges;
+}

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

@@ -0,0 +1,94 @@
+// 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 rising edge counter 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 edge 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 edges.
+ */
+class WPILIB_DLLEXPORT EdgeCounterFilter {
+ public:
+  /**
+   * Creates a new EdgeCounterFilter.
+   *
+   * @param requiredEdges The number of rising edges required before the output
+   *                      goes true.
+   * @param windowTime The maximum time window in which all required edges must
+   *               occur after the first rising edge.
+   */
+  explicit EdgeCounterFilter(int requiredEdges, units::second_t windowTime);
+
+  /**
+   * Applies the edge counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of edges have occurred within the time
+   *         window and the input is currently true; false otherwise.
+   */
+  bool Calculate(bool input);
+
+  /**
+   * Sets the time window duration.
+   *
+   * @param windowTime The maximum time window in which all required edges must
+   *               occur after the first rising edge.
+   */
+  constexpr void SetWindowTime(units::second_t windowTime) {
+    m_windowTime = windowTime;
+  }
+
+  /**
+   * Gets the time window duration.
+   *
+   * @return The maximum time window in which all required edges must occur
+   *         after the first rising edge.
+   */
+  constexpr units::second_t GetWindowTime() const { return m_windowTime; }
+
+  /**
+   * Sets the required number of edges.
+   *
+   * @param requiredEdges The number of rising edges required before the output
+   *                      goes true.
+   */
+  constexpr void SetRequiredEdges(int requiredEdges) {
+    m_requiredEdges = requiredEdges;
+  }
+
+  /**
+   * Gets the required number of edges.
+   *
+   * @return The number of rising edges required before the output goes true.
+   */
+  constexpr int GetRequiredEdges() const { return m_requiredEdges; }
+
+ private:
+  int m_requiredEdges;
+  units::second_t m_windowTime;
+
+  units::second_t m_firstEdgeTime;
+  int m_currentCount = 0;
+
+  bool m_lastInput = false;
+
+  void ResetTimer();
+
+  bool HasElapsed() const;
+};
+}  // namespace frc

wpimath/src/test/java/edu/wpi/first/math/filter/EdgeCounterFilterTest.java

@@ -0,0 +1,82 @@
+// 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.
+
+package edu.wpi.first.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class EdgeCounterFilterTest {
+  @BeforeEach
+  void setUp() {
+    WPIUtilJNI.enableMockTime();
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @AfterEach
+  void tearDown() {
+    WPIUtilJNI.setMockTime(0L);
+    WPIUtilJNI.disableMockTime();
+  }
+
+  @Test
+  void edgeCounterFilterActivatedTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertFalse(filter.calculate(true)); // First edge
+
+    WPIUtilJNI.setMockTime(50_000L);
+    assertFalse(filter.calculate(false)); // First edge ended
+
+    WPIUtilJNI.setMockTime(100_000L);
+    assertTrue(filter.calculate(true)); // Second edge
+
+    WPIUtilJNI.setMockTime(150_000L);
+    assertTrue(filter.calculate(true)); // Still true
+
+    WPIUtilJNI.setMockTime(250_000L);
+    assertTrue(filter.calculate(true)); // Still true
+
+    WPIUtilJNI.setMockTime(300_000L);
+    assertFalse(filter.calculate(false)); // Input false, should reset
+  }
+
+  @Test
+  void edgeCounterFilterExpiredTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertFalse(filter.calculate(true)); // First edge
+
+    WPIUtilJNI.setMockTime(50_000L);
+    filter.calculate(false); // First edge ended
+
+    WPIUtilJNI.setMockTime(250_000L);
+    assertFalse(filter.calculate(true)); // Second edge after window expired
+
+    WPIUtilJNI.setMockTime(300_000L);
+    assertFalse(filter.calculate(true)); // Still false
+  }
+
+  @Test
+  void edgeCounterFilterParamsTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertEquals(filter.getRequiredEdges(), 2);
+    assertEquals(filter.getWindowTime(), 0.2);
+
+    filter.setRequiredEdges(3);
+
+    assertEquals(filter.getRequiredEdges(), 3);
+
+    filter.setWindowTime(0.5);
+
+    assertEquals(filter.getWindowTime(), 0.5);
+  }
+}

wpimath/src/test/native/cpp/filter/EdgeCountFilterTest.cpp

@@ -0,0 +1,75 @@
+// 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 <gtest/gtest.h>
+#include <wpi/timestamp.h>
+
+#include "frc/filter/EdgeCountFilter.h"
+#include "units/time.h"
+
+static units::second_t now = 0_s;
+
+class EdgeCounterFilterTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+    now = 0_ms;
+  }
+
+  void TearDown() override {
+    now = 0_ms;
+    WPI_SetNowImpl(nullptr);
+  }
+};
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterActivated) {
+  frc::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // First edge ended
+
+  now = 100_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Second edge
+
+  now = 150_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 250_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // Input false, should reset
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterExpired) {
+  frc::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  filter.Calculate(false);  // First edge ended
+
+  now = 250_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Second edge after window expired
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Still false
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterParams) {
+  frc::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 2);
+  EXPECT_EQ(filter.GetWindowTime(), 0.2_s);
+
+  filter.SetRequiredEdges(3);
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 3);
+
+  filter.SetWindowTime(0.5_s);
+
+  EXPECT_EQ(filter.GetWindowTime(), 0.5_s);
+}