Added superstructure
Change-Id: I3fc6b2b555467fad231902fbc50af729b4b7157f
diff --git a/y2017_bot3/control_loops/superstructure/BUILD b/y2017_bot3/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..82b5d7a
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/BUILD
@@ -0,0 +1,47 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'superstructure_queue',
+ srcs = [
+ 'superstructure.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:profiled_subsystem_queue',
+ '//frc971/control_loops:queues',
+ ],
+)
+
+cc_library(
+ name = 'superstructure_lib',
+ srcs = [
+ 'superstructure.cc',
+ ],
+ hdrs = [
+ 'superstructure.h',
+ ],
+ deps = [
+ ':superstructure_queue',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:trapezoid_profile',
+ '//aos/common:math',
+ '//frc971/control_loops:profiled_subsystem',
+ '//frc971/control_loops:simple_capped_state_feedback_loop',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/zeroing',
+ ],
+)
+
+cc_binary(
+ name = 'superstructure',
+ srcs = [
+ 'superstructure_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':superstructure_lib',
+ ':superstructure_queue',
+ ],
+)
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
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.h b/y2017_bot3/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..79a901b
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure.h
@@ -0,0 +1,40 @@
+#ifndef Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ control_loops::SuperstructureQueue *my_superstructure =
+ &control_loops::superstructure_queue);
+
+ static constexpr double kOperatingVoltage = 12.0;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position * /*position*/,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status * /*status*/) override;
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017_bot3
+
+#endif // Y2017_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2017_bot3/control_loops/superstructure/superstructure.q b/y2017_bot3/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..91363c4
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure.q
@@ -0,0 +1,35 @@
+package y2017_bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Voltage to send to the rollers. Positive is sucking in.
+ float voltage_rollers;
+ bool fingers_out;
+ float hanger_voltage;
+ };
+
+ message Status {
+ };
+
+ message Position {
+ };
+
+ message Output {
+ float voltage_rollers;
+ bool fingers_out;
+ float hanger_voltage;
+ };
+
+ queue Goal goal;
+ queue Output output;
+ queue Status status;
+ queue Position position;
+};
+
+queue_group SuperstructureQueue superstructure_queue;
+
diff --git a/y2017_bot3/control_loops/superstructure/superstructure_main.cc b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..04a41a4
--- /dev/null
+++ b/y2017_bot3/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2017_bot3/control_loops/superstructure/superstructure.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2017_bot3::control_loops::superstructure::Superstructure superstructure;
+ superstructure.Run();
+ ::aos::Cleanup();
+ return 0;
+}