[wpimath] Move debouncer to filters (#3838)

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

@@ -0,0 +1,91 @@
+// 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.util.WPIUtilJNI;
+
+/**
+ * A simple debounce filter for boolean streams. Requires that the boolean change value from
+ * baseline for a specified period of time before the filtered value changes.
+ */
+public class Debouncer {
+  public enum DebounceType {
+    kRising,
+    kFalling,
+    kBoth
+  }
+
+  private final double m_debounceTimeSeconds;
+  private final DebounceType m_debounceType;
+  private boolean m_baseline;
+
+  private double m_prevTimeSeconds;
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounceTime The number of seconds the value must change from baseline for the filtered
+   *     value to change.
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  public Debouncer(double debounceTime, DebounceType type) {
+    m_debounceTimeSeconds = debounceTime;
+    m_debounceType = type;
+
+    resetTimer();
+
+    switch (m_debounceType) {
+      case kBoth: // fall-through
+      case kRising:
+        m_baseline = false;
+        break;
+      case kFalling:
+        m_baseline = true;
+        break;
+      default:
+        throw new IllegalArgumentException("Invalid debounce type!");
+    }
+  }
+
+  /**
+   * Creates a new Debouncer. Baseline value defaulted to "false."
+   *
+   * @param debounceTime The number of seconds the value must change from baseline for the filtered
+   *     value to change.
+   */
+  public Debouncer(double debounceTime) {
+    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.
+   *
+   * @param input The current value of the input stream.
+   * @return The debounced value of the input stream.
+   */
+  public boolean calculate(boolean input) {
+    if (input == m_baseline) {
+      resetTimer();
+    }
+
+    if (hasElapsed()) {
+      if (m_debounceType == DebounceType.kBoth) {
+        m_baseline = input;
+        resetTimer();
+      }
+      return input;
+    } else {
+      return m_baseline;
+    }
+  }
+}

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

@@ -0,0 +1,45 @@
+// 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/Debouncer.h"
+
+using namespace frc;
+
+Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
+    : m_debounceTime(debounceTime), m_debounceType(type) {
+  switch (type) {
+    case DebounceType::kBoth:  // fall-through
+    case DebounceType::kRising:
+      m_baseline = false;
+      break;
+    case DebounceType::kFalling:
+      m_baseline = true;
+      break;
+  }
+  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) {
+    ResetTimer();
+  }
+
+  if (HasElapsed()) {
+    if (m_debounceType == DebounceType::kBoth) {
+      m_baseline = input;
+      ResetTimer();
+    }
+    return input;
+  } else {
+    return m_baseline;
+  }
+}

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

@@ -0,0 +1,51 @@
+// 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/timestamp.h>
+
+#include "units/time.h"
+
+namespace frc {
+/**
+ * A simple debounce filter for boolean streams.  Requires that the boolean
+ * change value from baseline for a specified period of time before the filtered
+ * value changes.
+ */
+class WPILIB_DLLEXPORT Debouncer {
+ public:
+  enum DebounceType { kRising, kFalling, kBoth };
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounceTime The number of seconds the value must change from
+   *                     baseline for the filtered value to change.
+   * @param type         Which type of state change the debouncing will be
+   *                     performed on.
+   */
+  explicit Debouncer(units::second_t debounceTime,
+                     DebounceType type = DebounceType::kRising);
+
+  /**
+   * Applies the debouncer to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return The debounced value of the input stream.
+   */
+  bool Calculate(bool input);
+
+ private:
+  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

@@ -0,0 +1,70 @@
+// 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.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 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);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    WPIUtilJNI.setMockTime(1000000L);
+
+    assertTrue(debouncer.calculate(true));
+  }
+
+  @Test
+  void debounceFallingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kFalling);
+
+    debouncer.calculate(true);
+    assertTrue(debouncer.calculate(false));
+
+    WPIUtilJNI.setMockTime(1000000L);
+
+    assertFalse(debouncer.calculate(false));
+
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @Test
+  void debounceBothTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    WPIUtilJNI.setMockTime(1000000L);
+
+    assertTrue(debouncer.calculate(true));
+    assertTrue(debouncer.calculate(false));
+
+    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;