diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java index 65c6145f6b..fc8b3b1bbe 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java +++ b/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. + * + *

This is useful for implementing "double-click" style functionality. + * + *

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()); + } + }); + } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp index 6afdbb2b2c..6716fe6bc2 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -7,6 +7,7 @@ #include #include +#include #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(); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index 9e86b1b700..c3b3b76c27 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/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. + * + *

This is useful for implementing "double-click" style functionality. + * + *

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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/EdgeCounterFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/EdgeCounterFilter.java new file mode 100644 index 0000000000..7a4d80e296 --- /dev/null +++ b/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. + * + *

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. + */ +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; + } +} diff --git a/wpimath/src/main/native/cpp/filter/EdgeCountFilter.cpp b/wpimath/src/main/native/cpp/filter/EdgeCountFilter.cpp new file mode 100644 index 0000000000..dfa5e47ff4 --- /dev/null +++ b/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; +} diff --git a/wpimath/src/main/native/include/frc/filter/EdgeCountFilter.h b/wpimath/src/main/native/include/frc/filter/EdgeCountFilter.h new file mode 100644 index 0000000000..3fb682d24c --- /dev/null +++ b/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 + +#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 diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/EdgeCounterFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/EdgeCounterFilterTest.java new file mode 100644 index 0000000000..9da4c59cd2 --- /dev/null +++ b/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); + } +} diff --git a/wpimath/src/test/native/cpp/filter/EdgeCountFilterTest.cpp b/wpimath/src/test/native/cpp/filter/EdgeCountFilterTest.cpp new file mode 100644 index 0000000000..a6ae79756c --- /dev/null +++ b/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 +#include + +#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(); }); + 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); +}