Add debouncer to y2018 superstructure
Change-Id: Id53ded166c5fc918357ec37d3e79fdc57ba71e1b
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 1d52ab4..8fcba95 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -61,3 +61,24 @@
"//aos/linux_code:init",
],
)
+
+cc_library(
+ name = "debouncer",
+ hdrs = [
+ "debouncer.h",
+ ],
+ srcs = [
+ "debouncer.cc",
+ ],
+)
+
+cc_test(
+ name = "debouncer_test",
+ srcs = [
+ "debouncer_test.cc",
+ ],
+ deps = [
+ ":debouncer",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/debouncer.cc b/y2018/control_loops/superstructure/debouncer.cc
new file mode 100644
index 0000000..40b730d
--- /dev/null
+++ b/y2018/control_loops/superstructure/debouncer.cc
@@ -0,0 +1,25 @@
+#include "y2018/control_loops/superstructure/debouncer.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+void Debouncer::Update(bool new_state) {
+ // If the incoming state is different from the one we have stored, increment
+ // the counter.
+ if (new_state != current_state_) {
+ consistent_count_++;
+ } else {
+ consistent_count_ = 0;
+ }
+
+ // If we have reached the number required to change the state, change it.
+ if (consistent_count_ >= inputs_before_change_) {
+ current_state_ = new_state;
+ consistent_count_ = 0;
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/debouncer.h b/y2018/control_loops/superstructure/debouncer.h
new file mode 100644
index 0000000..ad46b51
--- /dev/null
+++ b/y2018/control_loops/superstructure/debouncer.h
@@ -0,0 +1,44 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_DEBOUNCER_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_DEBOUNCER_H_
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+// Ensures that a certain number of states of a certain type are recieved before
+// the actual state is changed.
+class Debouncer {
+ public:
+ // Parameters:
+ // - initial_state: the initial state of the debouncer. (assigned to
+ // current_state)
+ // - inputs_before_change: the number of inputs of the same type (true or
+ // false) required before the debouncer state is changed.
+ Debouncer(bool initial_state, int inputs_before_change)
+ : current_state_(initial_state),
+ inputs_before_change_(inputs_before_change) {}
+
+ // Updates the debounder state with a new input value.
+ void Update(bool new_state);
+
+ // Retrieves the current debouncer state.
+ bool current_state() const { return current_state_; }
+
+ private:
+ // Stores the current debouncer state.
+ bool current_state_;
+
+ // Stores the number of inputs of the same type (true or false) required
+ // before the debouncer state changes.
+ const int inputs_before_change_;
+
+ // Stores the temporary count of inputs of the same type. When this number
+ // reaches inputs_before_change_, the debouncer state changes.
+ int consistent_count_ = 0;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_DEBOUNCER_H_
diff --git a/y2018/control_loops/superstructure/debouncer_test.cc b/y2018/control_loops/superstructure/debouncer_test.cc
new file mode 100644
index 0000000..2f56d56
--- /dev/null
+++ b/y2018/control_loops/superstructure/debouncer_test.cc
@@ -0,0 +1,59 @@
+#include "y2018/control_loops/superstructure/debouncer.h"
+
+#include "gtest/gtest.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+// Tests that the debouncer behaves as it should. This tests the following:
+// - The debouncer changes its internal state after the desired number of
+// repeated inputs.
+// - The debouncer doesn't change its internal state before the desired number
+// of repeated inputs.
+TEST(DebouncerTest, Debouncer) {
+ Debouncer bouncer(false, 2);
+
+ bouncer.Update(true);
+ bouncer.Update(true);
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(false);
+
+ // Only one false, state shouldn't have changed.
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(false);
+ // Now there are two falses in a row, the state should've changed.
+ EXPECT_EQ(false, bouncer.current_state());
+}
+
+// Test that the debouncer will hold its state through a short-lived state
+// change.
+TEST(DebouncerTest, DebouncerLongSequence) {
+ Debouncer bouncer(false, 2);
+
+ bouncer.Update(true);
+
+ // Only one true, should still read false.
+ EXPECT_EQ(false, bouncer.current_state());
+
+ bouncer.Update(true);
+
+ // Two trues, should now read true.
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(false);
+
+ // Only one false, should still read true.
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(true);
+
+ EXPECT_EQ(true, bouncer.current_state());
+}
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018