Add y2019 superstructure, queues, constants.
Change-Id: I82b4bdfe77eff02e0dd7557491eb2e323bcf9de9
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..79d851a
--- /dev/null
+++ b/y2019/control_loops/superstructure/BUILD
@@ -0,0 +1,40 @@
+package(default_visibility = ["//visibility:public"])
+
+load("//aos/build:queues.bzl", "queue_library")
+
+queue_library(
+ name = "superstructure_queue",
+ srcs = [
+ "superstructure.q",
+ ],
+ deps = [
+ "//aos/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/controls:control_loop",
+ ]
+)
+
+cc_binary(
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ "//aos:init",
+ ]
+)
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..2f0832c
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,32 @@
+#include "y2019/control_loops/superstructure/superstructure.h"
+
+#include "aos/controls/control_loops.q.h"
+#include "frc971/control_loops/control_loops.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+Superstructure::Superstructure(
+ SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<SuperstructureQueue>(
+ superstructure_queue) {}
+
+void Superstructure::RunIteration(
+ const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) {
+ (void)unsafe_goal;
+ (void)position;
+ (void)output;
+ (void)status;
+
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..1faa6b4
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -0,0 +1,34 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/controls/control_loop.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ SuperstructureQueue *my_superstructure =
+ &superstructure_queue);
+
+ protected:
+ virtual void RunIteration(
+ const SuperstructureQueue::Goal *unsafe_goal,
+ const SuperstructureQueue::Position *position,
+ SuperstructureQueue::Output *output,
+ SuperstructureQueue::Status *status) override;
+
+ private:
+
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..2cd7238
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -0,0 +1,127 @@
+package y2019.control_loops.superstructure;
+
+import "aos/controls/control_loops.q";
+import "frc971/control_loops/profiled_subsystem.q";
+
+struct ElevatorGoal {
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ double height;
+
+ .frc971.ProfileParameters profile_params;
+};
+
+struct IntakeGoal {
+ // Positive is rollers intaking inward.
+ double roller_voltage;
+
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ double joint_angle;
+
+ .frc971.ProfileParameters profile_params;
+};
+
+struct SuctionGoal {
+ // True = open solenoid (apply suction)
+ // Top/bottom are when wrist is forward
+ bool top;
+ bool bottom;
+};
+
+struct StiltsGoal {
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the bottom
+ // of the middle wheels.
+ double height;
+
+ .frc971.ProfileParameters profile_params;
+};
+
+struct WristGoal {
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ double angle;
+ .frc971.ProfileParameters profile_params;
+};
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ ElevatorGoal elevator;
+ IntakeGoal intake;
+ SuctionGoal suction;
+ StiltsGoal stilts;
+ WristGoal wrist;
+ };
+
+ message Status {
+ // All subsystems know their location.
+ bool zeroed;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ bool estopped;
+
+ // Whether suction_pressure indicates cargo is held
+ bool has_piece;
+
+ // Status of each subsystem.
+ .frc971.control_loops.AbsoluteProfiledJointStatus elevator;
+ .frc971.control_loops.AbsoluteProfiledJointStatus wrist;
+ .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.AbsoluteProfiledJointStatus stilts;
+ };
+
+ message Position {
+ // Input from pressure sensor in psi
+ // 0 = current atmospheric pressure, negative = suction.
+ double suction_pressure;
+
+ // Position of the elevator, 0 at lowest position, positive when up.
+ .frc971.PotAndAbsolutePosition elevator;
+
+ // Position of wrist, 0 when up, positive is rotating toward the front,
+ // over the top.
+ .frc971.PotAndAbsolutePosition wrist;
+
+ // Position of the intake. 0 when rollers are retracted, positive extended.
+ .frc971.PotAndAbsolutePosition intake_joint;
+
+ // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
+ .frc971.PotAndAbsolutePosition stilts;
+ };
+
+ message Output {
+ // Voltage sent to motors moving elevator up/down. Positive is up.
+ double elevator_voltage;
+
+ // Voltage sent to wrist motors on elevator to rotate.
+ // Positive rotates over the top towards the front of the robot.
+ double wrist_voltage;
+
+ // Voltage sent to motors on intake joint. Positive extends rollers.
+ double intake_joint_voltage;
+
+ // Voltage sent to rollers on intake. Positive rolls inward.
+ double intake_roller_voltage;
+
+ // Voltage sent to motors to move stilts height. Positive moves robot upward.
+ double stilts_voltage;
+
+ // True opens solenoid (applies suction)
+ // Top/bottom are when wrist is toward the front of the robot
+ bool intake_suction_top;
+ bool intake_suction_bottom;
+
+ // Voltage sent to the vacuum pump motors.
+ double pump_voltage;
+ };
+
+ queue Goal goal;
+ queue Output output;
+ queue Status status;
+ queue Position position;
+};
+
+queue_group SuperstructureQueue superstructure_queue;
\ No newline at end of file
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..f3dd1ad
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2019/control_loops/superstructure/superstructure.h"
+
+#include "aos/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2019::control_loops::superstructure::Superstructure superstructure;
+ superstructure.Run();
+ ::aos::Cleanup();
+ return 0;
+}