Add Debouncer (#3590)

Supersedes #2358 with updates and cleanups.

Closes #2482 and closes #2487 because we shouldn't support both
time-based and count-based debouncing approaches.

Co-authored-by: oblarg <emichaelbarnett@gmail.com>

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

@@ -6,6 +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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -359,4 +360,23 @@ public class Trigger {
   public Trigger negate() {
     return new Trigger(() -> !get());
   }
+
+  /**
+   * Creates a new debounced trigger from this trigger - it will become active when this trigger has
+   * been active for longer than the specified period.
+   *
+   * @param seconds the debounce period
+   * @return the debounced trigger
+   */
+  public Trigger debounce(double seconds) {
+    return new Trigger(
+        new BooleanSupplier() {
+          Debouncer m_debouncer = new Debouncer(seconds);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_debouncer.calculate(get());
+          }
+        });
+  }
 }

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/button/Trigger.h"
 
+#include <frc/Debouncer.h>
+
 #include "frc2/command/InstantCommand.h"
 
 using namespace frc2;
@@ -136,3 +138,9 @@ Trigger Trigger::CancelWhenActive(Command* command) {
       });
   return *this;
 }
+
+Trigger Trigger::Debounce(units::second_t debounceTime) {
+  return Trigger([debouncer = frc::Debouncer(debounceTime), *this]() mutable {
+    return debouncer.Calculate(m_isActive());
+  });
+}

wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h

@@ -9,6 +9,7 @@
 #include <memory>
 #include <utility>
 
+#include <units/time.h>
 #include <wpi/span.h>
 
 #include "frc2/command/Command.h"
@@ -345,6 +346,15 @@ class Trigger {
     return Trigger([*this] { return !m_isActive(); });
   }
 
+  /**
+   * Creates a new debounced trigger from this trigger - it will become active
+   * when this trigger has been active for longer than the specified period.
+   *
+   * @param debounceTime the debounce period
+   * @return the debounced trigger
+   */
+  Trigger Debounce(units::second_t debounceTime);
+
  private:
   std::function<bool()> m_isActive;
 };

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java

@@ -12,6 +12,7 @@ import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 import static org.mockito.Mockito.when;
 
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.CommandTestBase;
@@ -172,4 +173,26 @@ class ButtonTest extends CommandTestBase {
     assertFalse(button1.negate().get());
     assertTrue(button1.and(button2.negate()).get());
   }
+
+  @Test
+  void debounceTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder commandHolder = new MockCommandHolder(true);
+    Command command = commandHolder.getMock();
+
+    InternalButton button = new InternalButton();
+    Trigger debounced = button.debounce(0.1);
+
+    debounced.whenActive(command);
+
+    button.setPressed(true);
+    scheduler.run();
+    verify(command, never()).schedule(true);
+
+    SimHooks.stepTiming(0.3);
+
+    button.setPressed(true);
+    scheduler.run();
+    verify(command).schedule(true);
+  }
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/button/ButtonTest.cpp

@@ -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.
 
-#include "CommandTestBase.h"
+#include <frc/simulation/SimHooks.h>
+
+#include "../CommandTestBase.h"
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/RunCommand.h"
 #include "frc2/command/WaitUntilCommand.h"
@@ -190,3 +192,19 @@ TEST_F(ButtonTest, RValueButton) {
   scheduler.Run();
   EXPECT_EQ(counter, 1);
 }
+
+TEST_F(ButtonTest, DebounceTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool pressed = false;
+  RunCommand command([] {});
+
+  Trigger([&pressed] { return pressed; }).Debounce(100_ms).WhenActive(&command);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::StepTiming(300_ms);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}

wpilibc/src/main/native/cpp/Debouncer.cpp

@@ -0,0 +1,37 @@
+// 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"
+
+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;
+  }
+  m_timer.Start();
+}
+
+bool Debouncer::Calculate(bool input) {
+  if (input == m_baseline) {
+    m_timer.Reset();
+  }
+
+  if (m_timer.HasElapsed(m_debounceTime)) {
+    if (m_debounceType == DebounceType::kBoth) {
+      m_baseline = input;
+      m_timer.Reset();
+    }
+    return input;
+  } else {
+    return m_baseline;
+  }
+}

wpilibc/src/main/native/include/frc/Debouncer.h

@@ -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.
+
+#pragma once
+
+#include <units/time.h>
+
+#include "frc/Timer.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 Debouncer {
+ public:
+  enum DebounceType { kRising, kFalling, kBoth };
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounce 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:
+  frc::Timer m_timer;
+  units::second_t m_debounceTime;
+  bool m_baseline;
+  DebounceType m_debounceType;
+};
+}  // namespace frc

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

@@ -0,0 +1,48 @@
+// 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));
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java

@@ -0,0 +1,79 @@
+// 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.wpilibj;
+
+/**
+ * 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 Timer m_timer = new Timer();
+  private final double m_debounceTime;
+  private final DebounceType m_debounceType;
+  private boolean m_baseline;
+
+  /**
+   * 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_debounceTime = debounceTime;
+    m_debounceType = type;
+    m_timer.start();
+
+    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);
+  }
+
+  /**
+   * 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) {
+      m_timer.reset();
+    }
+
+    if (m_timer.hasElapsed(m_debounceTime)) {
+      if (m_debounceType == DebounceType.kBoth) {
+        m_baseline = input;
+        m_timer.reset();
+      }
+      return input;
+    } else {
+      return m_baseline;
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java

@@ -0,0 +1,54 @@
+// 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.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.Test;
+
+public class DebouncerTest {
+  @Test
+  void debounceRisingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kRising);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    SimHooks.stepTiming(0.1);
+
+    assertTrue(debouncer.calculate(true));
+  }
+
+  @Test
+  void debounceFallingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kFalling);
+
+    debouncer.calculate(true);
+    assertTrue(debouncer.calculate(false));
+
+    SimHooks.stepTiming(0.1);
+
+    assertFalse(debouncer.calculate(false));
+  }
+
+  @Test
+  void debounceBothTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    SimHooks.stepTiming(0.1);
+
+    assertTrue(debouncer.calculate(true));
+    assertTrue(debouncer.calculate(false));
+
+    SimHooks.stepTiming(0.1);
+
+    assertFalse(debouncer.calculate(false));
+  }
+}