Add debouncer to y2018 superstructure
Change-Id: Id53ded166c5fc918357ec37d3e79fdc57ba71e1b
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