Prevent intaked balls from being spat accidentally
Intaking from the other side spits an intaked ball out.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I8bc0431431905c2691f8a5b8a5998fc21d2b1629
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 7e7a735..238eb0d 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -226,14 +226,18 @@
constexpr double kIntakePosition = -0.02;
constexpr size_t kIntakeCounterIterations = 25;
- // Extend the intakes and spin the rollers
- if (data.IsPressed(kIntakeFrontOut)) {
+ // 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.
+ if (data.IsPressed(kIntakeFrontOut) &&
+ !superstructure_status_fetcher_->back_intake_has_ball()) {
intake_front_pos = kIntakePosition;
transfer_roller_speed = kTransferRollerSpeed;
intake_front_counter_ = kIntakeCounterIterations;
intake_back_counter_ = 0;
- } else if (data.IsPressed(kIntakeBackOut)) {
+ } else if (data.IsPressed(kIntakeBackOut) &&
+ !superstructure_status_fetcher_->front_intake_has_ball()) {
intake_back_pos = kIntakePosition;
transfer_roller_speed = -kTransferRollerSpeed;