diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 453c680..f55e1ad 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -23,8 +23,30 @@
         "superstructure.h",
     ],
     deps = [
+        ":collision_avoidance",
         ":superstructure_queue",
         "//aos/controls:control_loop",
+        "//y2019:constants",
+    ],
+)
+
+cc_test(
+    name = "superstructure_lib_test",
+    srcs = [
+        "superstructure_lib_test.cc",
+    ],
+    deps = [
+        ":superstructure_lib",
+        ":superstructure_queue",
+        "//aos:math",
+        "//aos:queues",
+        "//aos/controls:control_loop_test",
+        "//aos/testing:googletest",
+        "//aos/time",
+        "//frc971/control_loops:capped_test_plant",
+        "//frc971/control_loops:position_sensor_sim",
+        "//frc971/control_loops:team_number_test_environment",
+        "//y2019/control_loops/superstructure/intake:intake_plants",
     ],
 )
 
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index 7a26b90..ed6787f 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -128,8 +128,8 @@
   }
 
   if (unsafe_goal) {
-    const double wrist_goal = unsafe_goal->wrist.angle;
-    const double intake_goal = unsafe_goal->intake.joint_angle;
+    const double wrist_goal = unsafe_goal->wrist.unsafe_goal;
+    const double intake_goal = unsafe_goal->intake.unsafe_goal;
 
     // Compute if we need to move the intake.
     const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 92a1a70..17f9fdc 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -36,20 +36,20 @@
         was_collided = avoidance.IsCollided(&status);
       }
 
-      safe_goal.wrist.angle =
-          ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+      safe_goal.wrist.unsafe_goal =
+          ::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
                       avoidance.max_wrist_goal());
 
-      safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+      safe_goal.elevator.unsafe_goal = ::std::max(unsafe_goal.elevator.unsafe_goal,
                                              avoidance.min_elevator_goal());
 
-      safe_goal.intake.joint_angle =
-          ::aos::Clip(unsafe_goal.intake.joint_angle,
+      safe_goal.intake.unsafe_goal =
+          ::aos::Clip(unsafe_goal.intake.unsafe_goal,
                       avoidance.min_intake_goal(), avoidance.max_intake_goal());
 
-      LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
-      LimitedMove(&status.elevator.position, safe_goal.elevator.height);
-      LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+      LimitedMove(&status.wrist.position, safe_goal.wrist.unsafe_goal);
+      LimitedMove(&status.elevator.position, safe_goal.elevator.unsafe_goal);
+      LimitedMove(&status.intake.position, safe_goal.intake.unsafe_goal);
       if (IsMoving()) {
         break;
       }
@@ -78,9 +78,9 @@
 
   void CheckGoals() {
     // check to see if we reached the goals
-    ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
-    ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
-    ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+    ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+    ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position, 0.001);
+    ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
   }
 
  private:
@@ -101,9 +101,9 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // in.
-  unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeInAngle - avoidance.kEpsIntake;
 
   // sets the status position messgaes to be have the elevator at the bottom
@@ -123,9 +123,9 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // out.
-  unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   // sets the status position messgaes to be have the elevator at the half way
@@ -152,9 +152,9 @@
 
   // sets the status position messgaes to be have the elevator at the half way
   // with the intake in and the wrist middle front
-  unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   Iterate();
@@ -174,9 +174,9 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // in.
-  unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeInAngle - avoidance.kEpsIntake;
 
   Iterate();
@@ -190,10 +190,10 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // out.
-  unsafe_goal.wrist.angle =
+  unsafe_goal.wrist.unsafe_goal =
       avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   // sets the status position messgaes to be have the elevator at the half way
@@ -213,10 +213,10 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // out.
-  unsafe_goal.wrist.angle =
+  unsafe_goal.wrist.unsafe_goal =
       avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   // sets the status position messgaes to be have the elevator at the half way
@@ -236,9 +236,9 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // out.
-  unsafe_goal.wrist.angle = 4.0;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = 4.0;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   // sets the status position messgaes to be have the elevator at the half way
@@ -249,10 +249,10 @@
 
   Iterate();
 
-  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
   ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
               status.elevator.position, 0.001);
-  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
 }
 
 // Unreasonable Wrist Goal
@@ -260,9 +260,9 @@
   // changes the goals to be in the position where the angle is low front and
   // the elevator is all the way at the bottom with the intake attempting to be
   // out.
-  unsafe_goal.wrist.angle = avoidance.kWristMinAngle;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
 
   // sets the status position messgaes to be have the elevator at the half way
@@ -274,18 +274,18 @@
 
   Iterate();
 
-  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
   ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
               status.elevator.position, 0.001);
-  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
 }
 
 // Fix Collision Wrist in Elevator
 TEST_F(CollisionAvoidanceTests, FixElevatorCollision) {
   // changes the goals
-  unsafe_goal.wrist.angle = 3.14;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = 3.14;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   // sets the status position messgaes
@@ -295,18 +295,18 @@
 
   Iterate();
 
-  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
   ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
               status.elevator.position, 0.001);
-  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
 }
 
 // Fix Collision Wrist in Intake
 TEST_F(CollisionAvoidanceTests, FixWristCollision) {
   // changes the goals
-  unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
 
   // sets the status position messgaes
@@ -317,17 +317,17 @@
 
   Iterate();
 
-  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
   ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
               status.elevator.position, 0.001);
-  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
 }
 // Fix Collision Wrist Above Elevator
 TEST_F(CollisionAvoidanceTests, FixWristElevatorCollision) {
   // changes the goals
-  unsafe_goal.wrist.angle = 0.0;
-  unsafe_goal.elevator.height = 0.0;
-  unsafe_goal.intake.joint_angle =
+  unsafe_goal.wrist.unsafe_goal = 0.0;
+  unsafe_goal.elevator.unsafe_goal = 0.0;
+  unsafe_goal.intake.unsafe_goal =
       avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
 
   // sets the status position messgaes
@@ -337,10 +337,10 @@
 
   Iterate();
 
-  ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
   ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
               status.elevator.position, 0.001);
-  ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+  ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
 }
 }  // namespace testing
 }  // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index e87ef6e..efa6ccb 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -2,6 +2,7 @@
 
 #include "aos/controls/control_loops.q.h"
 #include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
 
 namespace y2019 {
 namespace control_loops {
@@ -9,21 +10,60 @@
 
 Superstructure::Superstructure(::aos::EventLoop *event_loop,
                                const ::std::string &name)
-    : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name) {}
+    : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+      elevator_(constants::GetValues().elevator.subsystem_params),
+      wrist_(constants::GetValues().wrist.subsystem_params),
+      intake_(constants::GetValues().intake),
+      stilts_(constants::GetValues().stilts.subsystem_params) {}
 
-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;
-
+void Superstructure::RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+                                  const SuperstructureQueue::Position *position,
+                                  SuperstructureQueue::Output *output,
+                                  SuperstructureQueue::Status *status) {
   if (WasReset()) {
     LOG(ERROR, "WPILib reset, restarting\n");
+    elevator_.Reset();
+    wrist_.Reset();
+    intake_.Reset();
+    stilts_.Reset();
   }
+
+  elevator_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->elevator) : nullptr,
+                    &(position->elevator),
+                    output != nullptr ? &(output->elevator_voltage) : nullptr,
+                    &(status->elevator));
+
+  wrist_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->wrist) : nullptr,
+                 &(position->wrist),
+                 output != nullptr ? &(output->wrist_voltage) : nullptr,
+                 &(status->wrist));
+
+  intake_.Iterate(
+      unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+      &(position->intake_joint),
+      output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+      &(status->intake));
+
+  stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
+                  &(position->stilts),
+                  output != nullptr ? &(output->stilts_voltage) : nullptr,
+                  &(status->stilts));
+
+  status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
+                   status->intake.zeroed && status->stilts.zeroed;
+
+  status->estopped = status->elevator.estopped || status->wrist.estopped ||
+                     status->intake.estopped || status->stilts.estopped;
+
+  // TODO(theo) move these up when Iterate() is split
+  // update the goals
+  collision_avoidance_.UpdateGoal(status, unsafe_goal);
+
+  elevator_.set_min_position(collision_avoidance_.min_elevator_goal());
+  wrist_.set_min_position(collision_avoidance_.min_wrist_goal());
+  wrist_.set_max_position(collision_avoidance_.max_wrist_goal());
+  intake_.set_min_position(collision_avoidance_.min_intake_goal());
+  intake_.set_max_position(collision_avoidance_.max_intake_goal());
 }
 
 }  // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 0d6765e..22dbcd6 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,6 +2,9 @@
 #define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
 
 #include "aos/controls/control_loop.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
 #include "y2019/control_loops/superstructure/superstructure.q.h"
 
 namespace y2019 {
@@ -16,14 +19,41 @@
       const ::std::string &name =
           ".y2019.control_loops.superstructure.superstructure_queue");
 
+  using PotAndAbsoluteEncoderSubsystem =
+      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+          ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+          ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+  using AbsoluteEncoderSubsystem =
+      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+          ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+          ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+  const PotAndAbsoluteEncoderSubsystem &elevator() const {
+    return elevator_;
+  }
+  const PotAndAbsoluteEncoderSubsystem &wrist() const {
+    return wrist_;
+  }
+  const AbsoluteEncoderSubsystem &intake() const {
+    return intake_;
+  }
+  const PotAndAbsoluteEncoderSubsystem &stilts() const {
+    return stilts_;
+  }
+
  protected:
-  virtual void RunIteration(
-      const SuperstructureQueue::Goal *unsafe_goal,
-      const SuperstructureQueue::Position *position,
-      SuperstructureQueue::Output *output,
-      SuperstructureQueue::Status *status) override;
+  virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+                            const SuperstructureQueue::Position *position,
+                            SuperstructureQueue::Output *output,
+                            SuperstructureQueue::Status *status) override;
 
  private:
+  PotAndAbsoluteEncoderSubsystem elevator_;
+  PotAndAbsoluteEncoderSubsystem wrist_;
+  AbsoluteEncoderSubsystem intake_;
+  PotAndAbsoluteEncoderSubsystem stilts_;
+
+  CollisionAvoidance collision_avoidance_;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index cd004ac..a71f81a 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -3,25 +3,6 @@
 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
@@ -29,31 +10,29 @@
   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;
+    // Meters, 0 = lowest position - mechanical hard stop,
+    // positive = upward
+    .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal elevator;
+    // 0 = linkage on the sprocket is pointing straight up,
+    // positive = forward
+    .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal intake;
+    // 0 = Straight up parallel to elevator
+    // Positive rotates toward intake from 0
+    .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
+
+    // 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.
+    .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
+
+    // Positive is rollers intaking inward.
+    double roller_voltage;
+
     SuctionGoal suction;
-    StiltsGoal stilts;
-    WristGoal wrist;
   };
 
   message Status {
@@ -69,7 +48,7 @@
     // Status of each subsystem.
     .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
     .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
-    .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+    .frc971.control_loops.AbsoluteEncoderProfiledJointStatus intake;
     .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
   };
 
@@ -106,7 +85,8 @@
     // 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.
+    // Voltage sent to motors to move stilts height. Positive moves robot
+    // upward.
     double stilts_voltage;
 
     // True opens solenoid (applies suction)
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..de973c0
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,663 @@
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/controls/control_loop_test.h"
+#include "aos/queue.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
+#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
+#include "y2019/control_loops/superstructure/superstructure.h"
+#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+namespace {
+constexpr double kNoiseScalar = 0.01;
+}  // namespace
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::CappedTestPlant;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+    PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+  SuperstructureSimulation()
+      : elevator_plant_(
+            new CappedTestPlant(::y2019::control_loops::superstructure::
+                                    elevator::MakeElevatorPlant())),
+        elevator_pot_encoder_(M_PI * 2.0 *
+                              constants::Values::kElevatorEncoderRatio()),
+
+        wrist_plant_(new CappedTestPlant(
+            ::y2019::control_loops::superstructure::wrist::MakeWristPlant())),
+        wrist_pot_encoder_(M_PI * 2.0 *
+                           constants::Values::kWristEncoderRatio()),
+
+        intake_plant_(new CappedTestPlant(
+            ::y2019::control_loops::superstructure::intake::MakeIntakePlant())),
+        intake_pot_encoder_(M_PI * 2.0 *
+                            constants::Values::kIntakeEncoderRatio()),
+
+        stilts_plant_(new CappedTestPlant(
+            ::y2019::control_loops::superstructure::stilts::MakeStiltsPlant())),
+        stilts_pot_encoder_(M_PI * 2.0 *
+                            constants::Values::kStiltsEncoderRatio()),
+
+        superstructure_queue_(
+            ".y2019.control_loops.superstructure.superstructure_queue",
+            ".y2019.control_loops.superstructure.superstructure_queue.goal",
+            ".y2019.control_loops.superstructure.superstructure_queue.output",
+            ".y2019.control_loops.superstructure.superstructure_queue.status",
+            ".y2019.control_loops.superstructure.superstructure_queue."
+            "position") {
+    // Start the elevator out in the middle by default.
+    InitializeElevatorPosition(constants::Values::kElevatorRange().upper);
+
+    // Start the wrist out in the middle by default.
+    InitializeWristPosition(constants::Values::kWristRange().upper);
+
+    InitializeIntakePosition(constants::Values::kIntakeRange().upper);
+
+    // Start the stilts out in the middle by default.
+    InitializeStiltsPosition(constants::Values::kStiltsRange().lower);
+  }
+
+  void InitializeElevatorPosition(double start_pos) {
+    elevator_plant_->mutable_X(0, 0) = start_pos;
+    elevator_plant_->mutable_X(1, 0) = 0.0;
+
+    elevator_pot_encoder_.Initialize(
+        start_pos, kNoiseScalar, 0.0,
+        constants::GetValues()
+            .elevator.subsystem_params.zeroing_constants
+            .measured_absolute_position);
+  }
+
+  void InitializeWristPosition(double start_pos) {
+    wrist_plant_->mutable_X(0, 0) = start_pos;
+    wrist_plant_->mutable_X(1, 0) = 0.0;
+    wrist_pot_encoder_.Initialize(start_pos, kNoiseScalar, 0.0,
+                                  constants::GetValues()
+                                      .wrist.subsystem_params.zeroing_constants
+                                      .measured_absolute_position);
+  }
+
+  void InitializeIntakePosition(double start_pos) {
+    intake_plant_->mutable_X(0, 0) = start_pos;
+    intake_plant_->mutable_X(1, 0) = 0.0;
+
+    intake_pot_encoder_.Initialize(
+        start_pos, kNoiseScalar, 0.0,
+        constants::GetValues()
+            .intake.zeroing_constants.measured_absolute_position);
+  }
+
+  void InitializeStiltsPosition(double start_pos) {
+    stilts_plant_->mutable_X(0, 0) = start_pos;
+    stilts_plant_->mutable_X(1, 0) = 0.0;
+
+    stilts_pot_encoder_.Initialize(
+        start_pos, kNoiseScalar, 0.0,
+        constants::GetValues()
+            .stilts.subsystem_params.zeroing_constants
+            .measured_absolute_position);
+  }
+
+  // Sends a queue message with the position of the superstructure.
+  void SendPositionMessage() {
+    ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
+        superstructure_queue_.position.MakeMessage();
+
+    elevator_pot_encoder_.GetSensorValues(&position->elevator);
+    wrist_pot_encoder_.GetSensorValues(&position->wrist);
+    intake_pot_encoder_.GetSensorValues(&position->intake_joint);
+    stilts_pot_encoder_.GetSensorValues(&position->stilts);
+    position.Send();
+  }
+
+  double elevator_position() const { return elevator_plant_->X(0, 0); }
+  double elevator_velocity() const { return elevator_plant_->X(1, 0); }
+
+  double wrist_position() const { return wrist_plant_->X(0, 0); }
+  double wrist_velocity() const { return wrist_plant_->X(1, 0); }
+
+  double intake_position() const { return intake_plant_->X(0, 0); }
+  double intake_velocity() const { return intake_plant_->X(1, 0); }
+
+  double stilts_position() const { return stilts_plant_->X(0, 0); }
+  double stilts_velocity() const { return stilts_plant_->X(1, 0); }
+
+  // Sets the difference between the commanded and applied powers.
+  // This lets us test that the integrators work.
+
+  void set_elevator_voltage_offset(double voltage_offset) {
+    elevator_plant_->set_voltage_offset(voltage_offset);
+  }
+
+  void set_wrist_voltage_offset(double voltage_offset) {
+    wrist_plant_->set_voltage_offset(voltage_offset);
+  }
+
+  void set_intake_voltage_offset(double voltage_offset) {
+    intake_plant_->set_voltage_offset(voltage_offset);
+  }
+
+  void set_stilts_voltage_offset(double voltage_offset) {
+    stilts_plant_->set_voltage_offset(voltage_offset);
+  }
+
+  // Simulates the superstructure for a single timestep.
+  void Simulate() {
+    EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+    EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+
+    const double voltage_check_elevator =
+        (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+             superstructure_queue_.status->elevator.state) ==
+         PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+            ? constants::GetValues().elevator.subsystem_params.operating_voltage
+            : constants::GetValues().elevator.subsystem_params.zeroing_voltage;
+
+    const double voltage_check_wrist =
+        (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+             superstructure_queue_.status->wrist.state) ==
+         PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+            ? constants::GetValues().wrist.subsystem_params.operating_voltage
+            : constants::GetValues().wrist.subsystem_params.zeroing_voltage;
+
+    const double voltage_check_intake =
+        (static_cast<AbsoluteEncoderSubsystem::State>(
+             superstructure_queue_.status->intake.state) ==
+         AbsoluteEncoderSubsystem::State::RUNNING)
+            ? constants::GetValues().intake.operating_voltage
+            : constants::GetValues().intake.zeroing_voltage;
+
+    const double voltage_check_stilts =
+        (static_cast<PotAndAbsoluteEncoderSubsystem::State>(
+             superstructure_queue_.status->stilts.state) ==
+         PotAndAbsoluteEncoderSubsystem::State::RUNNING)
+            ? constants::GetValues().stilts.subsystem_params.operating_voltage
+            : constants::GetValues().stilts.subsystem_params.zeroing_voltage;
+
+    EXPECT_NEAR(superstructure_queue_.output->elevator_voltage, 0.0,
+                voltage_check_elevator);
+
+    EXPECT_NEAR(superstructure_queue_.output->wrist_voltage, 0.0,
+                voltage_check_wrist);
+
+    EXPECT_NEAR(superstructure_queue_.output->intake_joint_voltage, 0.0,
+                voltage_check_intake);
+
+    EXPECT_NEAR(superstructure_queue_.output->stilts_voltage, 0.0,
+                voltage_check_stilts);
+
+    ::Eigen::Matrix<double, 1, 1> elevator_U;
+    elevator_U << superstructure_queue_.output->elevator_voltage +
+                      elevator_plant_->voltage_offset();
+
+    ::Eigen::Matrix<double, 1, 1> wrist_U;
+    wrist_U << superstructure_queue_.output->wrist_voltage +
+                   wrist_plant_->voltage_offset();
+
+    ::Eigen::Matrix<double, 1, 1> intake_U;
+    intake_U << superstructure_queue_.output->intake_joint_voltage +
+                    intake_plant_->voltage_offset();
+
+    ::Eigen::Matrix<double, 1, 1> stilts_U;
+    stilts_U << superstructure_queue_.output->stilts_voltage +
+                    stilts_plant_->voltage_offset();
+
+    elevator_plant_->Update(elevator_U);
+    wrist_plant_->Update(wrist_U);
+    intake_plant_->Update(intake_U);
+    stilts_plant_->Update(stilts_U);
+
+    const double position_elevator = elevator_plant_->Y(0, 0);
+    const double position_wrist = wrist_plant_->Y(0, 0);
+    const double position_intake = intake_plant_->Y(0, 0);
+    const double position_stilts = stilts_plant_->Y(0, 0);
+
+    elevator_pot_encoder_.MoveTo(position_elevator);
+    wrist_pot_encoder_.MoveTo(position_wrist);
+    intake_pot_encoder_.MoveTo(position_intake);
+    stilts_pot_encoder_.MoveTo(position_stilts);
+
+    EXPECT_GE(position_elevator,
+              constants::Values::kElevatorRange().lower_hard);
+    EXPECT_LE(position_elevator,
+              constants::Values::kElevatorRange().upper_hard);
+
+    EXPECT_GE(position_wrist, constants::Values::kWristRange().lower_hard);
+    EXPECT_LE(position_wrist, constants::Values::kWristRange().upper_hard);
+
+    EXPECT_GE(position_intake, constants::Values::kIntakeRange().lower_hard);
+    EXPECT_LE(position_intake, constants::Values::kIntakeRange().upper_hard);
+
+    EXPECT_GE(position_stilts, constants::Values::kStiltsRange().lower_hard);
+    EXPECT_LE(position_stilts, constants::Values::kStiltsRange().upper_hard);
+  }
+
+ private:
+  ::std::unique_ptr<CappedTestPlant> elevator_plant_;
+  PositionSensorSimulator elevator_pot_encoder_;
+
+  ::std::unique_ptr<CappedTestPlant> wrist_plant_;
+  PositionSensorSimulator wrist_pot_encoder_;
+
+  ::std::unique_ptr<CappedTestPlant> intake_plant_;
+  PositionSensorSimulator intake_pot_encoder_;
+
+  ::std::unique_ptr<CappedTestPlant> stilts_plant_;
+  PositionSensorSimulator stilts_pot_encoder_;
+
+  SuperstructureQueue superstructure_queue_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+  SuperstructureTest()
+      : superstructure_queue_(
+            ".y2019.control_loops.superstructure.superstructure_queue",
+            ".y2019.control_loops.superstructure.superstructure_queue.goal",
+            ".y2019.control_loops.superstructure.superstructure_queue.output",
+            ".y2019.control_loops.superstructure.superstructure_queue.status",
+            ".y2019.control_loops.superstructure.superstructure_queue."
+            "position"),
+        superstructure_(&event_loop_) {
+    set_team_id(::frc971::control_loops::testing::kTeamNumber);
+  }
+
+  void VerifyNearGoal() {
+    superstructure_queue_.goal.FetchLatest();
+    superstructure_queue_.status.FetchLatest();
+
+    EXPECT_NEAR(superstructure_queue_.goal->elevator.unsafe_goal,
+                superstructure_queue_.status->elevator.position, 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->wrist.unsafe_goal,
+                superstructure_plant_.wrist_position(), 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->intake.unsafe_goal,
+                superstructure_queue_.status->intake.position, 0.001);
+    EXPECT_NEAR(superstructure_queue_.goal->stilts.unsafe_goal,
+                superstructure_plant_.stilts_position(), 0.001);
+  }
+
+  // Runs one iteration of the whole simulation.
+  void RunIteration(bool enabled = true) {
+    SendMessages(enabled);
+
+    superstructure_plant_.SendPositionMessage();
+    superstructure_.Iterate();
+    superstructure_plant_.Simulate();
+
+    TickTime(::std::chrono::microseconds(5050));
+  }
+
+  void CheckCollisions() {
+    superstructure_queue_.status.FetchLatest();
+    ASSERT_FALSE(
+        collision_avoidance_.IsCollided(superstructure_queue_.status.get()));
+  }
+
+  void WaitUntilZeroed() {
+    int i = 0;
+    do {
+      i++;
+      RunIteration();
+      superstructure_queue_.status.FetchLatest();
+      // 2 Seconds
+      ASSERT_LE(i, 2 * 1.0 / .00505);
+    } while (!superstructure_queue_.status.get()->zeroed);
+  }
+
+  // Runs iterations until the specified amount of simulated time has elapsed.
+  void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
+                  bool check_collisions = true) {
+    const auto start_time = monotonic_clock::now();
+    while (monotonic_clock::now() < start_time + run_for) {
+      const auto loop_start_time = monotonic_clock::now();
+      double begin_elevator_velocity =
+          superstructure_plant_.elevator_velocity();
+      double begin_wrist_velocity = superstructure_plant_.wrist_velocity();
+      double begin_intake_velocity = superstructure_plant_.intake_velocity();
+      double begin_stilts_velocity = superstructure_plant_.stilts_velocity();
+
+      RunIteration(enabled);
+      if (check_collisions) {
+        CheckCollisions();
+      }
+
+      const double loop_time = chrono::duration_cast<chrono::duration<double>>(
+                                   monotonic_clock::now() - loop_start_time)
+                                   .count();
+
+      const double elevator_acceleration =
+          (superstructure_plant_.elevator_velocity() -
+           begin_elevator_velocity) /
+          loop_time;
+      const double wrist_acceleration =
+          (superstructure_plant_.wrist_velocity() - begin_wrist_velocity) /
+          loop_time;
+      const double intake_acceleration =
+          (superstructure_plant_.intake_velocity() - begin_intake_velocity) /
+          loop_time;
+      const double stilts_acceleration =
+          (superstructure_plant_.stilts_velocity() - begin_stilts_velocity) /
+          loop_time;
+
+      EXPECT_GE(peak_elevator_acceleration_, elevator_acceleration);
+      EXPECT_LE(-peak_elevator_acceleration_, elevator_acceleration);
+      EXPECT_GE(peak_elevator_velocity_,
+                superstructure_plant_.elevator_velocity());
+      EXPECT_LE(-peak_elevator_velocity_,
+                superstructure_plant_.elevator_velocity());
+
+      EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
+      EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
+      EXPECT_GE(peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
+      EXPECT_LE(-peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
+
+      EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+      EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+      EXPECT_GE(peak_intake_velocity_, superstructure_plant_.intake_velocity());
+      EXPECT_LE(-peak_intake_velocity_,
+                superstructure_plant_.intake_velocity());
+
+      EXPECT_GE(peak_stilts_acceleration_, stilts_acceleration);
+      EXPECT_LE(-peak_stilts_acceleration_, stilts_acceleration);
+      EXPECT_GE(peak_stilts_velocity_, superstructure_plant_.stilts_velocity());
+      EXPECT_LE(-peak_stilts_velocity_,
+                superstructure_plant_.stilts_velocity());
+    }
+  }
+
+  void set_peak_elevator_acceleration(double value) {
+    peak_elevator_acceleration_ = value;
+  }
+  void set_peak_elevator_velocity(double value) {
+    peak_elevator_velocity_ = value;
+  }
+
+  void set_peak_wrist_acceleration(double value) {
+    peak_wrist_acceleration_ = value;
+  }
+  void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
+
+  void set_peak_intake_acceleration(double value) {
+    peak_intake_acceleration_ = value;
+  }
+  void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+
+  void set_peak_stilts_acceleration(double value) {
+    peak_stilts_acceleration_ = value;
+  }
+  void set_peak_stilts_velocity(double value) { peak_stilts_velocity_ = value; }
+
+  ::aos::ShmEventLoop event_loop_;
+  // Create a new instance of the test queue so that it invalidates the queue
+  // that it points to.  Otherwise, we will have a pointer to shared memory
+  // that is no longer valid.
+  SuperstructureQueue superstructure_queue_;
+
+  // Create a control loop and simulation.
+  Superstructure superstructure_;
+  SuperstructureSimulation superstructure_plant_;
+
+  // Creat a collision avoidance object
+  CollisionAvoidance collision_avoidance_;
+
+ private:
+  // The acceleration limits to check for while moving.
+  double peak_elevator_acceleration_ = 1e10;
+  double peak_wrist_acceleration_ = 1e10;
+  double peak_intake_acceleration_ = 1e10;
+  double peak_stilts_acceleration_ = 1e10;
+
+  // The velocity limits to check for while moving.
+  double peak_elevator_velocity_ = 1e10;
+  double peak_wrist_velocity_ = 1e10;
+  double peak_intake_velocity_ = 1e10;
+  double peak_stilts_velocity_ = 1e10;
+};
+
+// Tests that the superstructure does nothing when the goal is zero.
+TEST_F(SuperstructureTest, DoesNothing) {
+  superstructure_plant_.InitializeElevatorPosition(1.4);
+  superstructure_plant_.InitializeWristPosition(1.0);
+  superstructure_plant_.InitializeIntakePosition(1.1);
+  superstructure_plant_.InitializeStiltsPosition(0.1);
+
+  WaitUntilZeroed();
+
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+
+    goal->elevator.unsafe_goal = 1.4;
+    goal->wrist.unsafe_goal = 1.0;
+    goal->intake.unsafe_goal = 1.1;
+    goal->stilts.unsafe_goal = 0.1;
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(10));
+  VerifyNearGoal();
+
+  EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+}
+
+// Tests that loops can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+  // Set a reasonable goal.
+
+  superstructure_plant_.InitializeElevatorPosition(1.4);
+  superstructure_plant_.InitializeWristPosition(1.0);
+  superstructure_plant_.InitializeIntakePosition(1.1);
+  superstructure_plant_.InitializeStiltsPosition(0.1);
+
+  WaitUntilZeroed();
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->elevator.unsafe_goal = 1.4;
+    goal->elevator.profile_params.max_velocity = 1;
+    goal->elevator.profile_params.max_acceleration = 0.5;
+
+    goal->wrist.unsafe_goal = 1.0;
+    goal->wrist.profile_params.max_velocity = 1;
+    goal->wrist.profile_params.max_acceleration = 0.5;
+
+    goal->intake.unsafe_goal = 1.1;
+    goal->intake.profile_params.max_velocity = 1;
+    goal->intake.profile_params.max_acceleration = 0.5;
+
+    goal->stilts.unsafe_goal = 0.1;
+    goal->stilts.profile_params.max_velocity = 1;
+    goal->stilts.profile_params.max_acceleration = 0.5;
+
+    ASSERT_TRUE(goal.Send());
+  }
+
+  // Give it a lot of time to get there.
+  RunForTime(chrono::seconds(8));
+
+  VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+//
+// We are going to disable collision detection to make this easier to implement.
+TEST_F(SuperstructureTest, SaturationTest) {
+  // Zero it before we move.
+  WaitUntilZeroed();
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+    goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
+    goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+    goal->stilts.unsafe_goal = constants::Values::kStiltsRange().upper;
+
+    ASSERT_TRUE(goal.Send());
+  }
+  RunForTime(chrono::seconds(8));
+  VerifyNearGoal();
+
+  // Try a low acceleration move with a high max velocity and verify the
+  // acceleration is capped like expected.
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
+    goal->elevator.profile_params.max_velocity = 20.0;
+    goal->elevator.profile_params.max_acceleration = 0.1;
+
+    goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
+    goal->wrist.profile_params.max_velocity = 20.0;
+    goal->wrist.profile_params.max_acceleration = 0.1;
+
+    goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
+    goal->intake.profile_params.max_velocity = 20.0;
+    goal->intake.profile_params.max_acceleration = 0.1;
+
+    goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
+    goal->stilts.profile_params.max_velocity = 20.0;
+    goal->stilts.profile_params.max_acceleration = 0.1;
+
+    ASSERT_TRUE(goal.Send());
+  }
+  set_peak_elevator_velocity(23.0);
+  set_peak_elevator_acceleration(0.2);
+  set_peak_wrist_velocity(23.0);
+  set_peak_wrist_acceleration(0.2);
+  set_peak_intake_velocity(23.0);
+  set_peak_intake_acceleration(0.2);
+  set_peak_stilts_velocity(23.0);
+  set_peak_stilts_acceleration(0.2);
+
+  RunForTime(chrono::seconds(8));
+  VerifyNearGoal();
+
+  // Now do a high acceleration move with a low velocity limit.
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
+    goal->elevator.profile_params.max_velocity = 0.1;
+    goal->elevator.profile_params.max_acceleration = 10.0;
+
+    goal->wrist.unsafe_goal = constants::Values::kWristRange().lower;
+    goal->wrist.profile_params.max_velocity = 0.1;
+    goal->wrist.profile_params.max_acceleration = 10.0;
+
+    goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
+    goal->intake.profile_params.max_velocity = 0.1;
+    goal->intake.profile_params.max_acceleration = 10.0;
+
+    goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
+    goal->stilts.profile_params.max_velocity = 0.1;
+    goal->stilts.profile_params.max_acceleration = 10.0;
+  }
+  set_peak_elevator_velocity(0.2);
+  set_peak_elevator_acceleration(11.0);
+  set_peak_wrist_velocity(0.2);
+  set_peak_wrist_acceleration(11.0);
+  set_peak_intake_velocity(0.2);
+  set_peak_intake_acceleration(11.0);
+  set_peak_stilts_velocity(0.2);
+  set_peak_stilts_acceleration(11.0);
+
+  VerifyNearGoal();
+}
+
+// Tests if the robot zeroes properly... maybe redundant?
+TEST_F(SuperstructureTest, ZeroTest) {
+  superstructure_plant_.InitializeElevatorPosition(1.4);
+  superstructure_plant_.InitializeWristPosition(1.0);
+  superstructure_plant_.InitializeIntakePosition(1.1);
+  superstructure_plant_.InitializeStiltsPosition(0.1);
+
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+
+    goal->elevator.unsafe_goal = 1.4;
+    goal->elevator.profile_params.max_velocity = 1;
+    goal->elevator.profile_params.max_acceleration = 0.5;
+
+    goal->wrist.unsafe_goal = 1.0;
+    goal->wrist.profile_params.max_velocity = 1;
+    goal->wrist.profile_params.max_acceleration = 0.5;
+
+    goal->intake.unsafe_goal = 1.1;
+    goal->intake.profile_params.max_velocity = 1;
+    goal->intake.profile_params.max_acceleration = 0.5;
+
+    goal->stilts.unsafe_goal = 0.1;
+    goal->stilts.profile_params.max_velocity = 1;
+    goal->stilts.profile_params.max_acceleration = 0.5;
+
+    ASSERT_TRUE(goal.Send());
+  }
+  WaitUntilZeroed();
+  VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+  WaitUntilZeroed();
+  RunForTime(chrono::seconds(2));
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.elevator().state());
+
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.wrist().state());
+
+  EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.intake().state());
+
+  EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+            superstructure_.stilts().state());
+}
+
+// Move wrist front to back and see if we collide
+TEST_F(SuperstructureTest, CollisionTest) {
+  // Set a reasonable goal.
+  superstructure_plant_.InitializeElevatorPosition(
+      constants::Values::kElevatorRange().lower);
+  // 60 degrees forwards
+  superstructure_plant_.InitializeWristPosition(M_PI / 3.0);
+  superstructure_plant_.InitializeIntakePosition(
+      CollisionAvoidance::kIntakeOutAngle + CollisionAvoidance::kEpsIntake);
+  superstructure_plant_.InitializeStiltsPosition(0.1);
+
+  WaitUntilZeroed();
+  {
+    auto goal = superstructure_queue_.goal.MakeMessage();
+    goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
+    goal->wrist.unsafe_goal = -M_PI / 3.0;
+    goal->intake.unsafe_goal =
+        CollisionAvoidance::kIntakeInAngle - CollisionAvoidance::kEpsIntake;
+
+    ASSERT_TRUE(goal.Send());
+  }
+
+  // Give it a lot of time to get there.
+  RunForTime(chrono::seconds(20), true, true);
+
+  VerifyNearGoal();
+}
+
+}  // namespace testing
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2019