Merge all queues into one superstructure queue
Change-Id: I41c6b67160a53a7c0412e5a4cd3517b7c17dd44d
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..61fb001
--- /dev/null
+++ b/y2017/control_loops/superstructure/BUILD
@@ -0,0 +1,14 @@
+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:queues',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
new file mode 100644
index 0000000..a3b2715
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -0,0 +1,207 @@
+package y2017.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+struct JointState {
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ .frc971.EstimatorState estimator_state;
+};
+
+struct IntakeGoal {
+ // Zero on the intake is when the intake is retracted inside the robot,
+ // unable to intake. Positive is out.
+
+ // Goal distance of the intake.
+ double distance;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ .frc971.ProfileParameters profile_params;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ float voltage_rollers;
+};
+
+struct SerializerGoal {
+ // Serializer angular velocity goals in radians/second.
+ double angular_velocity;
+};
+
+struct TurretGoal {
+ // An azimuth angle of zero means the turrent faces toward the front of the
+ // robot where the intake is located. The angle increases when the turret
+ // turns clockwise, and decreases when the turrent turns counter-clockwise.
+ // These are from a top view above the robot.
+ double angle_azimuth;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ .frc971.ProfileParameters profile_params;
+};
+
+struct HoodGoal {
+ // Angle the hood is currently at
+ double angle_hood;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ .frc971.ProfileParameters profile_params;
+};
+
+struct ShooterGoal {
+ // Angular velocity goals in radians/second.
+ double angular_velocity;
+};
+
+struct IntakeStatus {
+ // Is it zeroed?
+ bool zeroed;
+
+ // Estimated position and velocities.
+ JointState joint_state;
+
+ // If true, we have aborted.
+ bool estopped;
+};
+
+struct SerializerStatus {
+ // The current average velocity in radians/second.
+ double avg_angular_velocity;
+
+ // The current instantaneous filtered velocity in radians/second.
+ double angular_velocity;
+
+ // True if the serializer is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ bool ready;
+
+ // If true, we have aborted.
+ bool estopped;
+};
+
+struct TurretStatus {
+ // Is the turret zeroed?
+ bool zeroed;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Estimate angles and angular velocities.
+ JointState azimuth;
+};
+
+struct HoodStatus {
+ // Is the turret zeroed?
+ bool zeroed;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Estimate angles and angular velocities.
+ JointState hood;
+};
+
+struct ShooterStatus {
+ // The current average velocity in radians/second.
+ double avg_angular_velocity;
+
+ // The current instantaneous filtered velocity in radians/second.
+ double angular_velocity;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ bool ready;
+
+ // If true, we have aborted.
+ bool estopped;
+};
+
+queue_group SuperstructureQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ IntakeGoal intake;
+ SerializerGoal serializer;
+ TurretGoal turret;
+ HoodGoal hood;
+ ShooterGoal shooter;
+ };
+
+ message Status {
+ // Are all the subsystems zeroed?
+ bool zeroed;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ bool estopped;
+
+ // Each subsystems status.
+ IntakeStatus intake;
+ SerializerStatus serializer;
+ TurretStatus turret;
+ HoodStatus hood;
+ ShooterStatus shooter;
+ };
+
+ message Position {
+ // Position of the intake, zero when the intake is in, positive when it is
+ // out.
+ .frc971.PotAndAbsolutePosition intake;
+
+ // Serializer angle in radians.
+ double theta_serializer;
+
+ // The sensor readings for the azimuth. The units and sign are defined the
+ // same as what's in the Goal message.
+ .frc971.PotAndAbsolutePosition azimuth_turret;
+
+ // Position of the hood in radians
+ double theta_hood;
+
+ // Shooter wheel angle in radians.
+ double theta_shooter;
+ };
+
+ message Output {
+ // Voltages for some of the subsystems.
+ float voltage_intake;
+ float voltage_serializer;
+ float voltage_shooter;
+
+ // Rollers on the intake.
+ float voltage_intake_rollers;
+ // Roller on the serializer
+ float voltage_serializer_rollers;
+
+ float voltage_azimuth_turret;
+ float voltage_hood;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group SuperstructureQueue superstructure_queue;