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
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