[wpimath] Move debouncer to filters (#3838)

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)
             }
 

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;

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

@@ -4,7 +4,7 @@
 
 #include "frc2/command/button/Trigger.h"
 
-#include <frc/Debouncer.h>
+#include <frc/filter/Debouncer.h>
 
 #include "frc2/command/InstantCommand.h"
 

wpilibc/src/test/native/cpp/DebouncerTest.cpp

@@ -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));
-}

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 {

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 {

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

@@ -4,9 +4,9 @@
 
 #pragma once
 
-#include <units/time.h>
+#include <wpi/timestamp.h>
 
-#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

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));
   }

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);
   }
 }

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 <wpi/timestamp.h>
+
+#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<uint64_t>(); });
+  }
+
+  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));
+}

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<uint64_t>(); });
+  }
+
+  void TearDown() override { WPI_SetNowImpl(nullptr); }
+};
+
+TEST_F(SlewRateLimiterTest, SlewRateLimit) {
   WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
 
   frc::SlewRateLimiter<units::meters> 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<uint64_t>(); });
-
+TEST_F(SlewRateLimiterTest, SlewRateNoLimit) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
   now += 1_s;