Added Vacuum class and test
Change-Id: I4c25946b71a0054791a2fc1305eb8ef509a147b1
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 187379b..3443d63 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -8,35 +8,6 @@
namespace control_loops {
namespace superstructure {
-void Superstructure::HandleSuction(const SuctionGoal *unsafe_goal,
- float suction_pressure,
- SuperstructureQueue::Output *output,
- bool *has_piece) {
- constexpr double kPumpVoltage = 12.0;
- constexpr double kPumpHasPieceVoltage = 8.0;
-
- // TODO(austin): Low pass filter on pressure.
- *has_piece = suction_pressure < 0.70;
-
- if (unsafe_goal && output) {
- const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
- if (evacuate) {
- vacuum_count_ = 200;
- }
- // TODO(austin): High speed pump a bit longer after we detect we have the
- // game piece.
- // 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 =
- (vacuum_count_ > 0) ? (*has_piece ? kPumpHasPieceVoltage : kPumpVoltage)
- : 0.0;
- output->intake_suction_top = unsafe_goal->top;
- output->intake_suction_bottom = unsafe_goal->bottom;
- }
- vacuum_count_ = ::std::max(0, vacuum_count_ - 1);
-}
-
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
@@ -77,8 +48,9 @@
output != nullptr ? &(output->stilts_voltage) : nullptr,
&(status->stilts));
- HandleSuction(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
- position->suction_pressure, output, &(status->has_piece));
+ vacuum_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
+ position->suction_pressure, output, &(status->has_piece),
+ event_loop());
status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
status->intake.zeroed && status->stilts.zeroed;