Suction Cups mini Control Loop
Change-Id: I85c26e5d674e5c1987d1d9a4617401ed8dfa7442
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index efa6ccb..6646f96 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -8,6 +8,18 @@
namespace control_loops {
namespace superstructure {
+void suction_cups(
+ const SuperstructureQueue::Goal *unsafe_goal,
+ SuperstructureQueue::Output *output) {
+ const double on_voltage = 12.0;
+
+ if(unsafe_goal && output) {
+ if(unsafe_goal->suction.top || unsafe_goal->suction.bottom) {
+ output->pump_voltage = on_voltage;
+ }
+ }
+}
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),