diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a10b61..af02d14 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,10 +78,11 @@
     '//aos/common/controls:control_loop',
     '//aos/common/util:trapezoid_profile',
     '//aos/common:math',
-    '//y2016/queues:ball_detector',
-    '//frc971/control_loops:state_feedback_loop',
+    '//frc971/control_loops:profiled_subsystem',
     '//frc971/control_loops:simple_capped_state_feedback_loop',
+    '//frc971/control_loops:state_feedback_loop',
     '//frc971/zeroing',
+    '//y2016/queues:ball_detector',
     '//y2016:constants',
   ],
 )
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 0c1ac6a..cb09b42 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -648,11 +648,11 @@
     }
   }
   arm_.set_max_voltage(
-      kill_shoulder_ ? 0.0 : max_voltage,
-      kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
-                                                : kShooterHangingVoltage)
-                     : max_voltage);
-  intake_.set_max_voltage(max_voltage);
+      {{kill_shoulder_ ? 0.0 : max_voltage,
+        kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage
+                                                  : kShooterHangingVoltage)
+                       : max_voltage}});
+  intake_.set_max_voltage({{max_voltage}});
 
   if (IsRunning() && !kill_shoulder_) {
     // We don't want lots of negative voltage when we are near the bellypan on
@@ -743,7 +743,7 @@
   status->shoulder.voltage_error = arm_.X_hat(4, 0);
   status->shoulder.calculated_velocity =
       (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
-  status->shoulder.estimator_state = arm_.ShoulderEstimatorState();
+  status->shoulder.estimator_state = arm_.EstimatorState(0);
 
   status->wrist.angle = arm_.X_hat(2, 0);
   status->wrist.angular_velocity = arm_.X_hat(3, 0);
@@ -754,7 +754,7 @@
   status->wrist.voltage_error = arm_.X_hat(5, 0);
   status->wrist.calculated_velocity =
       (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
-  status->wrist.estimator_state = arm_.WristEstimatorState();
+  status->wrist.estimator_state = arm_.EstimatorState(1);
 
   status->intake.angle = intake_.X_hat(0, 0);
   status->intake.angular_velocity = intake_.X_hat(1, 0);
@@ -766,7 +766,7 @@
   status->intake.calculated_velocity =
       (intake_.angle() - last_intake_angle_) / 0.005;
   status->intake.voltage_error = intake_.X_hat(2, 0);
-  status->intake.estimator_state = intake_.IntakeEstimatorState();
+  status->intake.estimator_state = intake_.EstimatorState(0);
   status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
 
   status->shoulder_controller_index = arm_.controller_index();
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index 7d4dea5..ba7576a 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -13,7 +13,6 @@
 namespace superstructure {
 
 using ::frc971::PotAndIndexPosition;
-using ::frc971::EstimatorState;
 
 namespace {
 double UseUnlessZero(double target_value, double default_value) {
@@ -30,148 +29,23 @@
 
 // Intake
 Intake::Intake()
-    : ProfiledSubsystem(
+    : ::frc971::control_loops::SingleDOFProfiledSubsystem(
           ::std::unique_ptr<
               ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
               new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
                   3, 1, 1>(::y2016::control_loops::superstructure::
-                               MakeIntegralIntakeLoop()))),
-      estimator_(constants::GetValues().intake.zeroing),
-      profile_(::aos::controls::kLoopFrequency) {
-  Y_.setZero();
-  offset_.setZero();
-  AdjustProfile(0.0, 0.0);
-}
-
-void Intake::UpdateIntakeOffset(double offset) {
-  const double doffset = offset - offset_(0, 0);
-  LOG(INFO, "Adjusting Intake offset from %f to %f\n", offset_(0, 0), offset);
-
-  loop_->mutable_X_hat()(0, 0) += doffset;
-  Y_(0, 0) += doffset;
-  loop_->mutable_R(0, 0) += doffset;
-
-  profile_.MoveGoal(doffset);
-  offset_(0, 0) = offset;
-
-  CapGoal("R", &loop_->mutable_R());
-}
-
-void Intake::Correct(PotAndIndexPosition position) {
-  estimator_.UpdateEstimate(position);
-
-  if (estimator_.error()) {
-    LOG(ERROR, "zeroing error with intake_estimator\n");
-    return;
-  }
-
-  if (!initialized_) {
-    if (estimator_.offset_ready()) {
-      UpdateIntakeOffset(estimator_.offset());
-      initialized_ = true;
-    }
-  }
-
-  if (!zeroed(0) && estimator_.zeroed()) {
-    UpdateIntakeOffset(estimator_.offset());
-    set_zeroed(0, true);
-  }
-
-  Y_ << position.encoder;
-  Y_ += offset_;
-  loop_->Correct(Y_);
-}
-
-void Intake::CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal) {
-  // Limit the goal to min/max allowable angles.
-  if ((*goal)(0, 0) > constants::Values::kIntakeRange.upper) {
-    LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
-        constants::Values::kIntakeRange.upper);
-    (*goal)(0, 0) = constants::Values::kIntakeRange.upper;
-  }
-  if ((*goal)(0, 0) < constants::Values::kIntakeRange.lower) {
-    LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
-        constants::Values::kIntakeRange.lower);
-    (*goal)(0, 0) = constants::Values::kIntakeRange.lower;
-  }
-}
-
-void Intake::ForceGoal(double goal) {
-  set_unprofiled_goal(goal);
-  loop_->mutable_R() = unprofiled_goal_;
-  loop_->mutable_next_R() = loop_->R();
-
-  profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
-}
-
-void Intake::set_unprofiled_goal(double unprofiled_goal) {
-  unprofiled_goal_(0, 0) = unprofiled_goal;
-  unprofiled_goal_(1, 0) = 0.0;
-  unprofiled_goal_(2, 0) = 0.0;
-  CapGoal("unprofiled R", &unprofiled_goal_);
-}
-
-void Intake::Update(bool disable) {
-  if (!disable) {
-    ::Eigen::Matrix<double, 2, 1> goal_state =
-        profile_.Update(unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
-
-    loop_->mutable_next_R(0, 0) = goal_state(0, 0);
-    loop_->mutable_next_R(1, 0) = goal_state(1, 0);
-    loop_->mutable_next_R(2, 0) = 0.0;
-    CapGoal("next R", &loop_->mutable_next_R());
-  }
-
-  loop_->Update(disable);
-
-  if (!disable && loop_->U(0, 0) != loop_->U_uncapped(0, 0)) {
-    profile_.MoveCurrentState(loop_->R().block<2, 1>(0, 0));
-  }
-}
-
-bool Intake::CheckHardLimits() {
-  // Returns whether hard limits have been exceeded.
-
-  if (angle() > constants::Values::kIntakeRange.upper_hard ||
-      angle() < constants::Values::kIntakeRange.lower_hard) {
-    LOG(ERROR, "Intake at %f out of bounds [%f, %f], ESTOPing\n", angle(),
-        constants::Values::kIntakeRange.lower_hard,
-        constants::Values::kIntakeRange.upper_hard);
-    return true;
-  }
-
-  return false;
-}
-
-void Intake::set_max_voltage(double voltage) {
-  loop_->set_max_voltage(0, voltage);
-}
-
-void Intake::AdjustProfile(double max_angular_velocity,
-                           double max_angular_acceleration) {
-  profile_.set_maximum_velocity(UseUnlessZero(max_angular_velocity, 10.0));
-  profile_.set_maximum_acceleration(
-      UseUnlessZero(max_angular_acceleration, 10.0));
-}
-
-void Intake::DoReset() {
-  estimator_.Reset();
-}
-
-EstimatorState Intake::IntakeEstimatorState() {
-  EstimatorState estimator_state;
-  ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
-
-  return estimator_state;
-}
+                               MakeIntegralIntakeLoop())),
+          constants::GetValues().intake.zeroing,
+          constants::Values::kIntakeRange, 10.0, 10.0) {}
 
 Arm::Arm()
-    : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
-          ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
+    : ProfiledSubsystem(
+          ::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+              ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+          {{constants::GetValues().shoulder.zeroing,
+            constants::GetValues().wrist.zeroing}}),
       shoulder_profile_(::aos::controls::kLoopFrequency),
-      wrist_profile_(::aos::controls::kLoopFrequency),
-      shoulder_estimator_(constants::GetValues().shoulder.zeroing),
-      wrist_estimator_(constants::GetValues().wrist.zeroing) {
+      wrist_profile_(::aos::controls::kLoopFrequency) {
   Y_.setZero();
   offset_.setZero();
   AdjustProfile(0.0, 0.0, 0.0, 0.0);
@@ -220,33 +94,34 @@
 
 void Arm::Correct(PotAndIndexPosition position_shoulder,
                   PotAndIndexPosition position_wrist) {
-  shoulder_estimator_.UpdateEstimate(position_shoulder);
-  wrist_estimator_.UpdateEstimate(position_wrist);
+  estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
+  estimators_[kWristIndex].UpdateEstimate(position_wrist);
 
   // Handle zeroing errors
-  if (shoulder_estimator_.error()) {
+  if (estimators_[kShoulderIndex].error()) {
     LOG(ERROR, "zeroing error with shoulder_estimator\n");
     return;
   }
-  if (wrist_estimator_.error()) {
+  if (estimators_[kWristIndex].error()) {
     LOG(ERROR, "zeroing error with wrist_estimator\n");
     return;
   }
 
   if (!initialized_) {
-    if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
-      UpdateShoulderOffset(shoulder_estimator_.offset());
-      UpdateWristOffset(wrist_estimator_.offset());
+    if (estimators_[kShoulderIndex].offset_ready() &&
+        estimators_[kWristIndex].offset_ready()) {
+      UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
+      UpdateWristOffset(estimators_[kWristIndex].offset());
       initialized_ = true;
     }
   }
 
-  if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
-    UpdateShoulderOffset(shoulder_estimator_.offset());
+  if (!zeroed(kShoulderIndex) && estimators_[kShoulderIndex].zeroed()) {
+    UpdateShoulderOffset(estimators_[kShoulderIndex].offset());
     set_zeroed(kShoulderIndex, true);
   }
-  if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
-    UpdateWristOffset(wrist_estimator_.offset());
+  if (!zeroed(kWristIndex) && estimators_[kWristIndex].zeroed()) {
+    UpdateWristOffset(estimators_[kWristIndex].offset());
     set_zeroed(kWristIndex, true);
   }
 
@@ -370,32 +245,6 @@
   }
 }
 
-void Arm::set_max_voltage(double shoulder_max_voltage,
-                          double wrist_max_voltage) {
-  loop_->set_max_voltage(0, shoulder_max_voltage);
-  loop_->set_max_voltage(1, wrist_max_voltage);
-}
-
-void Arm::DoReset() {
-  shoulder_estimator_.Reset();
-  wrist_estimator_.Reset();
-}
-
-EstimatorState Arm::ShoulderEstimatorState() {
-  EstimatorState estimator_state;
-  ::frc971::zeroing::PopulateEstimatorState(shoulder_estimator_,
-                                            &estimator_state);
-
-  return estimator_state;
-}
-
-EstimatorState Arm::WristEstimatorState() {
-  EstimatorState estimator_state;
-  ::frc971::zeroing::PopulateEstimatorState(wrist_estimator_, &estimator_state);
-
-  return estimator_state;
-}
-
 }  // namespace superstructure
 }  // namespace control_loops
 }  // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index adb40e0..7f7370f 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -4,13 +4,14 @@
 #include <memory>
 
 #include "aos/common/controls/control_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
 #include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
 
 #include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
 #include "y2016/control_loops/superstructure/integral_arm_plant.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
 
 namespace y2016 {
 namespace control_loops {
@@ -66,7 +67,8 @@
       const double overage_amount = U(0, 0) - max_voltage(0);
       mutable_U(0, 0) = max_voltage(0);
       const double coupled_amount =
-          (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) * overage_amount;
+          (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+          overage_amount;
       LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
       mutable_U(1, 0) += coupled_amount;
     }
@@ -96,149 +98,15 @@
   }
 };
 
-template <int number_of_states, int number_of_axes>
-class ProfiledSubsystem {
- public:
-  ProfiledSubsystem(
-      ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
-          number_of_states, number_of_axes, number_of_axes>>
-          loop)
-      : loop_(::std::move(loop)) {
-    zeroed_.fill(false);
-    unprofiled_goal_.setZero();
-  }
-
-  void Reset() {
-    zeroed_.fill(false);
-    DoReset();
-  }
-
-  // Returns the controller.
-  const StateFeedbackLoop<number_of_states, number_of_axes, number_of_axes>
-      &controller() const {
-    return *loop_;
-  }
-
-  int controller_index() const { return loop_->controller_index(); }
-
-  // Returns whether the estimators have been initialized and zeroed.
-  bool initialized() const { return initialized_; }
-
-  bool zeroed() const {
-    for (int i = 0; i < number_of_axes; ++i) {
-      if (!zeroed_[i]) {
-        return false;
-      }
-    }
-    return true;
-  }
-
-  bool zeroed(int index) const { return zeroed_[index]; };
-
-  // Returns the filtered goal.
-  const Eigen::Matrix<double, number_of_states, 1> &goal() const {
-    return loop_->R();
-  }
-  double goal(int row, int col) const { return loop_->R(row, col); }
-
-  // Returns the unprofiled goal.
-  const Eigen::Matrix<double, number_of_states, 1> &unprofiled_goal() const {
-    return unprofiled_goal_;
-  }
-  double unprofiled_goal(int row, int col) const {
-    return unprofiled_goal_(row, col);
-  }
-
-  // Returns the current state estimate.
-  const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
-    return loop_->X_hat();
-  }
-  double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
-
- protected:
-  void set_zeroed(int index, bool val) { zeroed_[index] = val; }
-
-  // TODO(austin): It's a bold assumption to assume that we will have the same
-  // number of sensors as axes.  So far, that's been fine.
-  ::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
-      number_of_states, number_of_axes, number_of_axes>>
-      loop_;
-
-  // The goal that the profile tries to reach.
-  Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
-
-  bool initialized_ = false;
-
- private:
-  ::std::array<bool, number_of_axes> zeroed_;
-
-  virtual void DoReset() = 0;
-};
-
-class Intake : public ProfiledSubsystem<3, 1> {
+class Intake : public ::frc971::control_loops::SingleDOFProfiledSubsystem {
  public:
   Intake();
-  // Returns whether an error has occured
-  bool error() const { return estimator_.error(); }
-
-  // Updates our estimator with the latest position.
-  void Correct(::frc971::PotAndIndexPosition position);
-  // Runs the controller and profile generator for a cycle.
-  void Update(bool disabled);
-  // Sets the maximum voltage that will be commanded by the loop.
-  void set_max_voltage(double voltage);
-
-  // Forces the current goal to the provided goal, bypassing the profiler.
-  void ForceGoal(double goal);
-  // Sets the unprofiled goal.  The profiler will generate a profile to go to
-  // this goal.
-  void set_unprofiled_goal(double unprofiled_goal);
-  // Limits our profiles to a max velocity and acceleration for proper motion.
-  void AdjustProfile(double max_angular_velocity,
-                     double max_angular_acceleration);
-
-  // Returns true if we have exceeded any hard limits.
-  bool CheckHardLimits();
-
-  // Returns the current internal estimator state for logging.
-  ::frc971::EstimatorState IntakeEstimatorState();
-
-  // Returns the requested intake voltage.
-  double intake_voltage() const { return loop_->U(0, 0); }
-
-  // Returns the current position.
-  double angle() const { return Y_(0, 0); }
-
-  // For testing:
-  // Triggers an estimator error.
-  void TriggerEstimatorError() { estimator_.TriggerError(); }
-
- private:
-  // Limits the provided goal to the soft limits.  Prints "name" when it fails
-  // to aid debugging.
-  void CapGoal(const char *name, Eigen::Matrix<double, 3, 1> *goal);
-
-  // Resets the internal state.
-  void DoReset() override;
-
-  void UpdateIntakeOffset(double offset);
-
-  ::frc971::zeroing::ZeroingEstimator estimator_;
-  aos::util::TrapezoidProfile profile_;
-
-  // Current measurement.
-  Eigen::Matrix<double, 1, 1> Y_;
-  // Current offset.  Y_ = offset_ + raw_sensor;
-  Eigen::Matrix<double, 1, 1> offset_;
 };
 
-class Arm : public ProfiledSubsystem<6, 2> {
+
+class Arm : public ::frc971::control_loops::ProfiledSubsystem<6, 2> {
  public:
   Arm();
-  // Returns whether an error has occured
-  bool error() const {
-    return shoulder_estimator_.error() || wrist_estimator_.error();
-  }
 
   // Updates our estimator with the latest position.
   void Correct(::frc971::PotAndIndexPosition position_shoulder,
@@ -259,7 +127,6 @@
                      double max_angular_acceleration_shoulder,
                      double max_angular_velocity_wrist,
                      double max_angular_acceleration_wrist);
-  void set_max_voltage(double shoulder_max_voltage, double wrist_max_voltage);
 
   void set_shoulder_asymetric_limits(double shoulder_min_voltage,
                                      double shoulder_max_voltage) {
@@ -269,10 +136,6 @@
   // Returns true if we have exceeded any hard limits.
   bool CheckHardLimits();
 
-  // Returns the current internal estimator state for logging.
-  ::frc971::EstimatorState ShoulderEstimatorState();
-  ::frc971::EstimatorState WristEstimatorState();
-
   // Returns the requested shoulder and wrist voltages.
   double shoulder_voltage() const { return loop_->U(0, 0); }
   double wrist_voltage() const { return loop_->U(1, 0); }
@@ -283,12 +146,9 @@
 
   // For testing:
   // Triggers an estimator error.
-  void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
+  void TriggerEstimatorError() { estimators_[0].TriggerError(); }
 
  private:
-  // Resets the internal state.
-  void DoReset() override;
-
   // Limits the provided goal to the soft limits.  Prints "name" when it fails
   // to aid debugging.
   void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);
@@ -300,7 +160,6 @@
   friend class testing::SuperstructureTest_DisabledGoalTest_Test;
 
   aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
-  ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;
 
   // Current measurement.
   Eigen::Matrix<double, 2, 1> Y_;