From aa9dfabde2fdaf32541697de2159878faa801fa1 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Tue, 28 Dec 2021 12:49:41 -0500 Subject: [PATCH] [wpimath] Move debouncer to filters (#3838) --- wpilibNewCommands/build.gradle | 1 + .../wpilibj2/command/button/Trigger.java | 2 +- .../cpp/frc2/command/button/Trigger.cpp | 2 +- wpilibc/src/test/native/cpp/DebouncerTest.cpp | 48 --------------- .../edu/wpi/first/math/filter}/Debouncer.java | 28 ++++++--- .../src/main/native/cpp/filter}/Debouncer.cpp | 18 ++++-- .../native/include/frc/filter}/Debouncer.h | 13 +++-- .../wpi/first/math/filter}/DebouncerTest.java | 28 +++++++-- .../math/filter/SlewRateLimiterTest.java | 22 ++++--- .../test/native/cpp/filter/DebouncerTest.cpp | 58 +++++++++++++++++++ .../native/cpp/filter/SlewRateLimiterTest.cpp | 15 +++-- 11 files changed, 150 insertions(+), 85 deletions(-) delete mode 100644 wpilibc/src/test/native/cpp/DebouncerTest.cpp rename {wpilibj/src/main/java/edu/wpi/first/wpilibj => wpimath/src/main/java/edu/wpi/first/math/filter}/Debouncer.java (79%) rename {wpilibc/src/main/native/cpp => wpimath/src/main/native/cpp/filter}/Debouncer.cpp (71%) rename {wpilibc/src/main/native/include/frc => wpimath/src/main/native/include/frc/filter}/Debouncer.h (87%) rename {wpilibj/src/test/java/edu/wpi/first/wpilibj => wpimath/src/test/java/edu/wpi/first/math/filter}/DebouncerTest.java (68%) create mode 100644 wpimath/src/test/native/cpp/filter/DebouncerTest.cpp diff --git a/wpilibNewCommands/build.gradle b/wpilibNewCommands/build.gradle index 8b4695b9b8..3c9f0c4dd7 100644 --- a/wpilibNewCommands/build.gradle +++ b/wpilibNewCommands/build.gradle @@ -75,6 +75,7 @@ model { if (it.component.name == "${nativeName}Dev") { lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared' project(':hal').addHalJniDependency(it) } 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 edf20f4d56..6d2c9a8de9 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 @@ -6,7 +6,7 @@ package edu.wpi.first.wpilibj2.command.button; import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; -import edu.wpi.first.wpilibj.Debouncer; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; 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 0801549bdc..dd41e1f9e5 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -4,7 +4,7 @@ #include "frc2/command/button/Trigger.h" -#include +#include #include "frc2/command/InstantCommand.h" diff --git a/wpilibc/src/test/native/cpp/DebouncerTest.cpp b/wpilibc/src/test/native/cpp/DebouncerTest.cpp deleted file mode 100644 index 379a6d514d..0000000000 --- a/wpilibc/src/test/native/cpp/DebouncerTest.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// 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/Debouncer.h" // NOLINT(build/include_order) - -#include "frc/simulation/SimHooks.h" -#include "gtest/gtest.h" - -using namespace frc; - -TEST(DebouncerTest, DebounceRising) { - Debouncer debouncer{20_ms}; - - debouncer.Calculate(false); - EXPECT_FALSE(debouncer.Calculate(true)); - - frc::sim::StepTiming(100_ms); - - EXPECT_TRUE(debouncer.Calculate(true)); -} - -TEST(DebouncerTest, DebounceFalling) { - Debouncer debouncer{20_ms, Debouncer::DebounceType::kFalling}; - - debouncer.Calculate(true); - EXPECT_TRUE(debouncer.Calculate(false)); - - frc::sim::StepTiming(100_ms); - - EXPECT_FALSE(debouncer.Calculate(false)); -} - -TEST(DebouncerTest, DebounceBoth) { - Debouncer debouncer{20_ms, Debouncer::DebounceType::kBoth}; - - debouncer.Calculate(false); - EXPECT_FALSE(debouncer.Calculate(true)); - - frc::sim::StepTiming(100_ms); - - EXPECT_TRUE(debouncer.Calculate(true)); - EXPECT_TRUE(debouncer.Calculate(false)); - - frc::sim::StepTiming(100_ms); - - EXPECT_FALSE(debouncer.Calculate(false)); -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java similarity index 79% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java rename to wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java index cef85aa0ef..8da45e9586 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java @@ -2,7 +2,9 @@ // 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.wpilibj; +package edu.wpi.first.math.filter; + +import edu.wpi.first.util.WPIUtilJNI; /** * A simple debounce filter for boolean streams. Requires that the boolean change value from @@ -15,11 +17,12 @@ public class Debouncer { kBoth } - private final Timer m_timer = new Timer(); - private final double m_debounceTime; + private final double m_debounceTimeSeconds; private final DebounceType m_debounceType; private boolean m_baseline; + private double m_prevTimeSeconds; + /** * Creates a new Debouncer. * @@ -28,9 +31,10 @@ public class Debouncer { * @param type Which type of state change the debouncing will be performed on. */ public Debouncer(double debounceTime, DebounceType type) { - m_debounceTime = debounceTime; + m_debounceTimeSeconds = debounceTime; m_debounceType = type; - m_timer.start(); + + resetTimer(); switch (m_debounceType) { case kBoth: // fall-through @@ -55,6 +59,14 @@ public class Debouncer { this(debounceTime, DebounceType.kRising); } + private void resetTimer() { + m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6; + } + + private boolean hasElapsed() { + return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds; + } + /** * Applies the debouncer to the input stream. * @@ -63,13 +75,13 @@ public class Debouncer { */ public boolean calculate(boolean input) { if (input == m_baseline) { - m_timer.reset(); + resetTimer(); } - if (m_timer.hasElapsed(m_debounceTime)) { + if (hasElapsed()) { if (m_debounceType == DebounceType.kBoth) { m_baseline = input; - m_timer.reset(); + resetTimer(); } return input; } else { diff --git a/wpilibc/src/main/native/cpp/Debouncer.cpp b/wpimath/src/main/native/cpp/filter/Debouncer.cpp similarity index 71% rename from wpilibc/src/main/native/cpp/Debouncer.cpp rename to wpimath/src/main/native/cpp/filter/Debouncer.cpp index eb402cdf80..4e909a2503 100644 --- a/wpilibc/src/main/native/cpp/Debouncer.cpp +++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp @@ -2,7 +2,7 @@ // 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/Debouncer.h" +#include "frc/filter/Debouncer.h" using namespace frc; @@ -17,18 +17,26 @@ Debouncer::Debouncer(units::second_t debounceTime, DebounceType type) m_baseline = true; break; } - m_timer.Start(); + ResetTimer(); +} + +void Debouncer::ResetTimer() { + m_prevTime = units::microsecond_t(wpi::Now()); +} + +bool Debouncer::HasElapsed() const { + return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime; } bool Debouncer::Calculate(bool input) { if (input == m_baseline) { - m_timer.Reset(); + ResetTimer(); } - if (m_timer.HasElapsed(m_debounceTime)) { + if (HasElapsed()) { if (m_debounceType == DebounceType::kBoth) { m_baseline = input; - m_timer.Reset(); + ResetTimer(); } return input; } else { diff --git a/wpilibc/src/main/native/include/frc/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h similarity index 87% rename from wpilibc/src/main/native/include/frc/Debouncer.h rename to wpimath/src/main/native/include/frc/filter/Debouncer.h index 8f583f01db..73071021c7 100644 --- a/wpilibc/src/main/native/include/frc/Debouncer.h +++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h @@ -4,9 +4,9 @@ #pragma once -#include +#include -#include "frc/Timer.h" +#include "units/time.h" namespace frc { /** @@ -14,7 +14,7 @@ namespace frc { * change value from baseline for a specified period of time before the filtered * value changes. */ -class Debouncer { +class WPILIB_DLLEXPORT Debouncer { public: enum DebounceType { kRising, kFalling, kBoth }; @@ -38,9 +38,14 @@ class Debouncer { bool Calculate(bool input); private: - frc::Timer m_timer; units::second_t m_debounceTime; bool m_baseline; DebounceType m_debounceType; + + units::second_t m_prevTime; + + void ResetTimer(); + + bool HasElapsed() const; }; } // namespace frc diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java similarity index 68% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java rename to wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java index 05d91aaadf..b6334de009 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java @@ -2,15 +2,29 @@ // 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.wpilibj; +package edu.wpi.first.math.filter; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.wpilibj.simulation.SimHooks; +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 DebouncerTest { + @BeforeEach + void setUp() { + WPIUtilJNI.enableMockTime(); + WPIUtilJNI.setMockTime(0L); + } + + @AfterEach + void tearDown() { + WPIUtilJNI.setMockTime(0L); + WPIUtilJNI.disableMockTime(); + } + @Test void debounceRisingTest() { var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kRising); @@ -18,7 +32,7 @@ class DebouncerTest { debouncer.calculate(false); assertFalse(debouncer.calculate(true)); - SimHooks.stepTiming(0.1); + WPIUtilJNI.setMockTime(1000000L); assertTrue(debouncer.calculate(true)); } @@ -30,9 +44,11 @@ class DebouncerTest { debouncer.calculate(true); assertTrue(debouncer.calculate(false)); - SimHooks.stepTiming(0.1); + WPIUtilJNI.setMockTime(1000000L); assertFalse(debouncer.calculate(false)); + + WPIUtilJNI.setMockTime(0L); } @Test @@ -42,12 +58,12 @@ class DebouncerTest { debouncer.calculate(false); assertFalse(debouncer.calculate(true)); - SimHooks.stepTiming(0.1); + WPIUtilJNI.setMockTime(1000000L); assertTrue(debouncer.calculate(true)); assertTrue(debouncer.calculate(false)); - SimHooks.stepTiming(0.1); + WPIUtilJNI.setMockTime(2000000L); assertFalse(debouncer.calculate(false)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java index f8921082c7..e7ec644ca8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java @@ -8,28 +8,34 @@ import static org.junit.jupiter.api.Assertions.assertEquals; 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 SlewRateLimiterTest { + @BeforeEach + void setUp() { + WPIUtilJNI.enableMockTime(); + WPIUtilJNI.setMockTime(0L); + } + + @AfterEach + void tearDown() { + WPIUtilJNI.setMockTime(0L); + WPIUtilJNI.disableMockTime(); + } + @Test void slewRateLimitTest() { - WPIUtilJNI.enableMockTime(); - var limiter = new SlewRateLimiter(1); WPIUtilJNI.setMockTime(1000000L); assertTrue(limiter.calculate(2) < 2); - - WPIUtilJNI.setMockTime(0L); } @Test void slewRateNoLimitTest() { - WPIUtilJNI.enableMockTime(); - var limiter = new SlewRateLimiter(1); WPIUtilJNI.setMockTime(1000000L); assertEquals(limiter.calculate(0.5), 0.5); - - WPIUtilJNI.setMockTime(0L); } } diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp new file mode 100644 index 0000000000..2f64908af0 --- /dev/null +++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp @@ -0,0 +1,58 @@ +// 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 "frc/filter/Debouncer.h" +#include "gtest/gtest.h" +#include "units/time.h" + +static units::second_t now = 0_s; + +class DebouncerTest : public ::testing::Test { + protected: + void SetUp() override { + WPI_SetNowImpl([] { return units::microsecond_t{now}.to(); }); + } + + void TearDown() override { WPI_SetNowImpl(nullptr); } +}; + +TEST_F(DebouncerTest, DebounceRising) { + frc::Debouncer debouncer{20_ms}; + + debouncer.Calculate(false); + EXPECT_FALSE(debouncer.Calculate(true)); + + now += 1_s; + + EXPECT_TRUE(debouncer.Calculate(true)); +} + +TEST_F(DebouncerTest, DebounceFalling) { + frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kFalling}; + + debouncer.Calculate(true); + EXPECT_TRUE(debouncer.Calculate(false)); + + now += 1_s; + + EXPECT_FALSE(debouncer.Calculate(false)); +} + +TEST_F(DebouncerTest, DebounceBoth) { + frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth}; + + debouncer.Calculate(false); + EXPECT_FALSE(debouncer.Calculate(true)); + + now += 1_s; + + EXPECT_TRUE(debouncer.Calculate(true)); + EXPECT_TRUE(debouncer.Calculate(false)); + + now += 1_s; + + EXPECT_FALSE(debouncer.Calculate(false)); +} diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp index d2c0bae5e7..4896073ba3 100644 --- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp +++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp @@ -12,7 +12,16 @@ static units::second_t now = 0_s; -TEST(SlewRateLimiterTest, SlewRateLimit) { +class SlewRateLimiterTest : public ::testing::Test { + protected: + void SetUp() override { + WPI_SetNowImpl([] { return units::microsecond_t{now}.to(); }); + } + + void TearDown() override { WPI_SetNowImpl(nullptr); } +}; + +TEST_F(SlewRateLimiterTest, SlewRateLimit) { WPI_SetNowImpl([] { return units::microsecond_t{now}.to(); }); frc::SlewRateLimiter limiter(1_mps); @@ -22,9 +31,7 @@ TEST(SlewRateLimiterTest, SlewRateLimit) { EXPECT_LT(limiter.Calculate(2_m), 2_m); } -TEST(SlewRateLimiterTest, SlewRateNoLimit) { - WPI_SetNowImpl([] { return units::microsecond_t{now}.to(); }); - +TEST_F(SlewRateLimiterTest, SlewRateNoLimit) { frc::SlewRateLimiter limiter(1_mps); now += 1_s;