Initial hood implementation.
- Zeroing works, control loop converges.
Change-Id: Icf28f17f7623dbe565379ae14c56e699c2631a8e
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..424af9a
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,35 @@
+#include "y2017/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/hood/hood.h"
+
+namespace y2017 {
+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) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ hood_.Reset();
+ }
+
+ hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
+ &(position->hood),
+ output != nullptr ? &(output->voltage_hood) : nullptr,
+ &(status->hood));
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017