Added Vacuum class and test

Change-Id: I4c25946b71a0054791a2fc1305eb8ef509a147b1
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
new file mode 100644
index 0000000..21acb2e
--- /dev/null
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -0,0 +1,57 @@
+#include "y2019/control_loops/superstructure/vacuum.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double Vacuum::kPumpVoltage;
+constexpr double Vacuum::kPumpHasPieceVoltage;
+constexpr aos::monotonic_clock::duration Vacuum::kTimeAtHigherVoltage;
+constexpr aos::monotonic_clock::duration Vacuum::kTimeToKeepPumpRunning;
+
+void Vacuum::Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
+                     SuperstructureQueue::Output *output, bool *has_piece,
+                     aos::EventLoop *event_loop) {
+  auto monotonic_now = event_loop->monotonic_now();
+  bool low_pump_voltage = false;
+  bool no_goal_for_a_bit = false;
+
+  // implement a simple low-pass filter on the pressure
+  filtered_pressure_ = kSuctionAlpha * suction_pressure +
+                       (1 - kSuctionAlpha) * filtered_pressure_;
+
+  *has_piece = filtered_pressure_ < kVacuumThreshold;
+
+  if (*has_piece && !had_piece_) {
+    time_at_last_acquisition_ = monotonic_now;
+  }
+
+  // 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;
+  no_goal_for_a_bit =
+      monotonic_now > time_at_last_evacuate_goal_ + kTimeToKeepPumpRunning;
+
+  if (unsafe_goal && output) {
+    const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
+    if (evacuate) {
+      time_at_last_evacuate_goal_ = monotonic_now;
+    }
+
+    // Once the vacuum evacuates, the pump speeds up because there is no
+    // resistance.  So, we want to turn it down to save the pump from
+    // overheating.
+    output->pump_voltage =
+        (no_goal_for_a_bit) ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage
+                                                    : kPumpVoltage);
+
+    output->intake_suction_top = unsafe_goal->top;
+    output->intake_suction_bottom = unsafe_goal->bottom;
+  }
+  had_piece_ = *has_piece;
+}
+
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2019