Add intake control loop.
Change-Id: I504e4b11c0d8424249e2544da517748a657ddef8
diff --git a/bot3/control_loops/intake/intake.cc b/bot3/control_loops/intake/intake.cc
new file mode 100644
index 0000000..935730b
--- /dev/null
+++ b/bot3/control_loops/intake/intake.cc
@@ -0,0 +1,36 @@
+#include "bot3/control_loops/intake/intake.h"
+
+#include "bot3/control_loops/intake/intake.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+Intake::Intake(control_loops::IntakeQueue *intake)
+ : aos::controls::ControlLoop<control_loops::IntakeQueue>(intake) {}
+
+void Intake::RunIteration(
+ const control_loops::IntakeQueue::Goal *goal,
+ const control_loops::IntakeQueue::Position * /*position*/,
+ control_loops::IntakeQueue::Output *output,
+ control_loops::IntakeQueue::Status * /*status*/) {
+
+ if (output != nullptr) {
+ output->Zero();
+
+ const int16_t intake_movement = goal->movement;
+
+ if (intake_movement > 0) {
+ // Suck.
+ output->intake = kIntakeVoltageFullPower;
+ } else if (intake_movement < 0) {
+ // Spit.
+ output->intake = -kIntakeVoltageFullPower;
+ } else {
+ // Stationary.
+ output->intake = 0.0;
+ }
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3