commit f70107e10720d772ac8ede3e96d72ea5ae1fc339
author Ravago Jones <ravagojones@gmail.com> Tue Apr 12 15:05:56 2022 -0700
committer Ravago Jones <ravagojones@gmail.com> Fri Apr 15 13:22:16 2022 -0800
tree ab48d7e01a257cdb604e80fa69356c2bacb6251d
parent d2b64538ed5e0ef5d2bf8b10e77de36565b8dd61

Make spit button smarter

Adds a third spit button that spits the side that has a ball.

Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I6842750d157d1c131f59bdf1abcf906e34f568a8

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 8ff1e1c..21a38d3 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -239,7 +239,7 @@
     back_intake_beambreak_timer_ = timestamp;
   }
 
-  // Check if we're either spitting of have lost the ball.
+  // Check if we're either spitting or have lost the ball.
   if ((transfer_roller_speed < 0.0 && front_intake_has_ball_) ||
       timestamp >
           front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {

y2022/joystick_reader.cc

diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 05f3008..6066209 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -64,8 +64,9 @@
 
 const ButtonLocation kIntakeFrontOut(4, 10);
 const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kSpitFront(3, 3);
-const ButtonLocation kSpitBack(3, 1);
+const ButtonLocation kSpitFront(4, 8);
+const ButtonLocation kSpitBack(4, 7);
+const ButtonLocation kSpit(3, 3);
 
 const ButtonLocation kRedLocalizerReset(4, 14);
 const ButtonLocation kBlueLocalizerReset(4, 13);
@@ -232,6 +233,13 @@
     constexpr double kIntakePosition = -0.12;
     constexpr size_t kIntakeCounterIterations = 25;
 
+    if (data.PosEdge(kSpit)) {
+      last_front_intake_has_ball_ =
+          superstructure_status_fetcher_->front_intake_has_ball();
+      last_back_intake_has_ball_ =
+          superstructure_status_fetcher_->back_intake_has_ball();
+    }
+
     // Extend the intakes and spin the rollers.
     // Don't let this happen if there is a ball in the other intake, because
     // that would spit this one out.
@@ -249,10 +257,12 @@
 
       intake_back_counter_ = kIntakeCounterIterations;
       intake_front_counter_ = 0;
-    } else if (data.IsPressed(kSpitFront)) {
+    } else if (data.IsPressed(kSpitFront) ||
+               (data.IsPressed(kSpit) && last_front_intake_has_ball_)) {
       transfer_roller_speed = -kTransferRollerSpeed;
       intake_front_counter_ = 0;
-    } else if (data.IsPressed(kSpitBack)) {
+    } else if (data.IsPressed(kSpitBack) ||
+               (data.IsPressed(kSpit) && last_back_intake_has_ball_)) {
       transfer_roller_speed = kTransferRollerSpeed;
       intake_back_counter_ = 0;
     }
@@ -361,6 +371,9 @@
 
   size_t intake_front_counter_ = 0;
   size_t intake_back_counter_ = 0;
+
+  bool last_front_intake_has_ball_ = false;
+  bool last_back_intake_has_ball_ = false;
 };
 
 }  // namespace joysticks