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 866d701f3c..029d2fd93f 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,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()); + } + }); + } } 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 322420202f..0801549bdc 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -4,6 +4,8 @@ #include "frc2/command/button/Trigger.h" +#include + #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()); + }); +} 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 67a194f5de..3c1d8f90ad 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h @@ -9,6 +9,7 @@ #include #include +#include #include #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 m_isActive; }; diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java index c4ba8275b9..1a4f05d450 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java +++ b/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); + } } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/ButtonTest.cpp similarity index 91% rename from wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp rename to wpilibNewCommands/src/test/native/cpp/frc2/command/button/ButtonTest.cpp index 158e26b4d7..c8671529f8 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp +++ b/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 + +#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)); +} diff --git a/wpilibc/src/main/native/cpp/Debouncer.cpp b/wpilibc/src/main/native/cpp/Debouncer.cpp new file mode 100644 index 0000000000..eb402cdf80 --- /dev/null +++ b/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; + } +} diff --git a/wpilibc/src/main/native/include/frc/Debouncer.h b/wpilibc/src/main/native/include/frc/Debouncer.h new file mode 100644 index 0000000000..53bc12744e --- /dev/null +++ b/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 + +#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 diff --git a/wpilibc/src/test/native/cpp/DebouncerTest.cpp b/wpilibc/src/test/native/cpp/DebouncerTest.cpp new file mode 100644 index 0000000000..379a6d514d --- /dev/null +++ b/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)); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java new file mode 100644 index 0000000000..cef85aa0ef --- /dev/null +++ b/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; + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java new file mode 100644 index 0000000000..5a51f6c0fb --- /dev/null +++ b/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)); + } +}