Delay setting has_piece in the vacuum.
This makes it such that has_piece corresponds to when we are applying
full voltage to the vacuum so we delay lifting the ball out of the
intake until then.
Change-Id: Ib50acabbd013dd14cbfc8e871e4493c8fc6e10c0
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 21acb2e..d4c1c6b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -20,16 +20,17 @@
filtered_pressure_ = kSuctionAlpha * suction_pressure +
(1 - kSuctionAlpha) * filtered_pressure_;
- *has_piece = filtered_pressure_ < kVacuumThreshold;
+ const bool new_has_piece = filtered_pressure_ < kVacuumThreshold;
- if (*has_piece && !had_piece_) {
+ if (new_has_piece && !had_piece_) {
time_at_last_acquisition_ = monotonic_now;
}
+ *has_piece =
+ monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage &&
+ new_has_piece;
// if we've had the piece for enought time, go to lower pump_voltage
- low_pump_voltage =
- *has_piece &&
- monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage;
+ low_pump_voltage = *has_piece;
no_goal_for_a_bit =
monotonic_now > time_at_last_evacuate_goal_ + kTimeToKeepPumpRunning;
@@ -49,7 +50,7 @@
output->intake_suction_top = unsafe_goal->top;
output->intake_suction_bottom = unsafe_goal->bottom;
}
- had_piece_ = *has_piece;
+ had_piece_ = new_has_piece;
}
} // namespace superstructure