Added Vacuum class and test
Change-Id: I4c25946b71a0054791a2fc1305eb8ef509a147b1
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
new file mode 100644
index 0000000..599e2de
--- /dev/null
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -0,0 +1,55 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
+
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "aos/controls/control_loop.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+class Vacuum {
+ public:
+ Vacuum() {}
+ void Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
+ SuperstructureQueue::Output *output, bool *has_piece,
+ aos::EventLoop *event_loop);
+
+
+ // Voltage to the vaccum pump when we are attempting to acquire a piece
+ static constexpr double kPumpVoltage = 12.0;
+
+ // Voltage to the vaccum pump when we have a piece
+ static constexpr double kPumpHasPieceVoltage = 8.0;
+
+ // Time to continue at the higher pump voltage after getting a gamepiece
+ static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
+ std::chrono::milliseconds(200);
+
+ // Time to continue the pump after getting a no suck goal
+ static constexpr aos::monotonic_clock::duration kTimeToKeepPumpRunning =
+ std::chrono::milliseconds(1000);
+
+ private:
+ bool had_piece_ = false;
+ aos::monotonic_clock::time_point time_at_last_evacuate_goal_ =
+ aos::monotonic_clock::epoch();
+ aos::monotonic_clock::time_point time_at_last_acquisition_ =
+ aos::monotonic_clock::epoch();
+ double filtered_pressure_ = 1.0;
+
+ static constexpr double kVacuumThreshold = 0.70;
+
+ static constexpr double kFilterTimeConstant = 0.1;
+ static constexpr double dt = .00505;
+ static constexpr double kSuctionAlpha =
+ dt * (1 - kFilterTimeConstant) / (kFilterTimeConstant);
+
+ DISALLOW_COPY_AND_ASSIGN(Vacuum);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_