commit c09b157ca91036194fd92069ae7b5d9507fdfb35
author Austin Schuh <austin.linux@gmail.com> Fri Oct 29 23:08:15 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Nov 06 21:33:13 2021 -0700
tree 58e42f1ed99bdd8bd6f1bc1028b56938faac5d30
parent 09b6e63345898bf04ffcb2e853bbeffeea3e684c

Debounce the winch button

It is quite dangerous since it locks the turret.  Make it a lot harder
to enter by accident.

Change-Id: I0fe04a1d05fd4d0d9f03e1b93557b31f399c0f95
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2020/joystick_reader.cc

diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index b92126a..bf6cd9f 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -137,8 +137,6 @@
     }
   }
 
-  bool latched_climbing_ = false;
-
   void HandleTeleop(
       const ::frc971::input::driver_station::Data &data) override {
     superstructure_status_fetcher_.Fetch();
@@ -216,17 +214,31 @@
     }
 
     if (data.IsPressed(kWinch)) {
+      ++winch_counter_;
+    } else {
+      winch_counter_ = 0;
+    }
+
+    if (winch_counter_ > 5 || (winch_counter_ > 0 && latched_climbing_)) {
       climber_speed = 12.0f;
       latched_climbing_ = true;
     }
 
     if (data.IsPressed(kUnWinch)) {
+      ++unwinch_counter_;
+    } else {
+      unwinch_counter_ = 0;
+    }
+
+    if (unwinch_counter_ > 10 || (unwinch_counter_ > 0 && latched_climbing_)) {
       climber_speed = -12.0f;
       latched_climbing_ = true;
     }
 
     if (data.IsPressed(kWinch) && data.IsPressed(kUnWinch)) {
       latched_climbing_ = false;
+      unwinch_counter_ = 0;
+      winch_counter_ = 0;
     }
 
     if (latched_climbing_) {
@@ -306,6 +318,11 @@
   ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
 
   ::aos::Fetcher<y2020::joysticks::Setpoint> setpoint_fetcher_;
+
+  bool latched_climbing_ = false;
+
+  size_t winch_counter_ = 0;
+  size_t unwinch_counter_ = 0;
 };
 
 }  // namespace joysticks