Added superstructure
Change-Id: I3fc6b2b555467fad231902fbc50af729b4b7157f
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.cc b/y2017_bot3/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..9cb069b
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,39 @@
+#include "y2017_bot3/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+namespace y2017_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+Superstructure::Superstructure(
+ control_loops::SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+ superstructure_queue) {}
+
+void Superstructure::RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position * /*position*/,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status * /*status*/) {
+ // Write out all the voltages.
+ if (output) {
+ output->voltage_rollers = 0.0;
+ output->hanger_voltage = 0.0;
+ output->fingers_out = false;
+ if (unsafe_goal) {
+ // Intake.
+ output->voltage_rollers = unsafe_goal->voltage_rollers;
+
+ output->fingers_out = unsafe_goal->fingers_out;
+
+ output->hanger_voltage = unsafe_goal->hanger_voltage;
+ }
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017_bot3