diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 8147fc2..c64fac1 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -631,6 +631,8 @@
   status->intake.estimator_state = intake_.IntakeEstimatorState();
   status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
 
+  status->shoulder_controller_index = arm_.controller_index();
+
   last_shoulder_angle_ = arm_.shoulder_angle();
   last_wrist_angle_ = arm_.wrist_angle();
   last_intake_angle_ = intake_.angle();
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 277e3ce..0883446 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -81,6 +81,8 @@
     JointState intake;
     JointState shoulder;
     JointState wrist;
+
+    int32_t shoulder_controller_index;
   };
 
   message Position {
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index b2d8aa7..0a0a825 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -162,7 +162,7 @@
 }
 
 Arm::Arm()
-    : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>(
+    : loop_(new ArmControlLoop(
           ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
       shoulder_profile_(::aos::controls::kLoopFrequency),
       wrist_profile_(::aos::controls::kLoopFrequency),
@@ -338,19 +338,14 @@
 void Arm::Update(bool disable) {
   if (!disable) {
     // Compute next goal.
-    ::Eigen::Matrix<double, 2, 1> goal_state_shoulder =
-        shoulder_profile_.Update(unprofiled_goal_(0, 0),
-                                 unprofiled_goal_(1, 0));
-    loop_->mutable_next_R(0, 0) = goal_state_shoulder(0, 0);
-    loop_->mutable_next_R(1, 0) = goal_state_shoulder(1, 0);
+    loop_->mutable_next_R().block<2, 1>(0, 0) = shoulder_profile_.Update(
+        unprofiled_goal_(0, 0), unprofiled_goal_(1, 0));
 
-    ::Eigen::Matrix<double, 2, 1> goal_state_wrist =
+    loop_->mutable_next_R().block<2, 1>(2, 0) =
         wrist_profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
-    loop_->mutable_next_R(2, 0) = goal_state_wrist(0, 0);
-    loop_->mutable_next_R(3, 0) = goal_state_wrist(1, 0);
 
-    loop_->mutable_next_R(4, 0) = unprofiled_goal_(4, 0);
-    loop_->mutable_next_R(5, 0) = unprofiled_goal_(5, 0);
+    loop_->mutable_next_R().block<2, 1>(4, 0) =
+        unprofiled_goal_.block<2, 1>(4, 0);
     CapGoal("next R", &loop_->mutable_next_R());
   }
 
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index b8c674f..b4fbead 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -10,6 +10,7 @@
 
 #include "frc971/zeroing/zeroing.h"
 #include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/integral_arm_plant.h"
 
 namespace y2016 {
 namespace control_loops {
@@ -105,6 +106,53 @@
   bool zeroed_ = false;
 };
 
+class ArmControlLoop
+    : public ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2> {
+ public:
+  ArmControlLoop(SimpleCappedStateFeedbackLoop<6, 2, 2> &&loop)
+      : SimpleCappedStateFeedbackLoop<6, 2, 2>(::std::move(loop)) {}
+
+  const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
+    const Eigen::Matrix<double, 2, 1> accelerating_ff =
+        controller(0).Kff * (next_R() - controller(0).plant.A() * R());
+    const Eigen::Matrix<double, 2, 1> accelerating_controller =
+        controller(0).K * error() + accelerating_ff;
+
+    const Eigen::Matrix<double, 2, 1> decelerating_ff =
+        controller(1).Kff * (next_R() - controller(1).plant.A() * R());
+    const Eigen::Matrix<double, 2, 1> decelerating_controller =
+        controller(1).K * error() + decelerating_ff;
+
+    const double bemf_voltage = X_hat(1, 0) / kV_shoulder;
+    bool use_accelerating_controller = true;
+    LOG(DEBUG, "Accelerating at %f, decel %f, bemf %f\n",
+        accelerating_controller(0, 0), accelerating_controller(1, 0),
+        bemf_voltage);
+    if (IsAccelerating(bemf_voltage, accelerating_controller(0, 0))) {
+      use_accelerating_controller = true;
+    } else {
+      use_accelerating_controller = false;
+    }
+    if (use_accelerating_controller) {
+      ff_U_ = accelerating_ff;
+      set_controller_index(0);
+      return accelerating_controller;
+    } else {
+      set_controller_index(1);
+      ff_U_ = decelerating_ff;
+      return decelerating_controller;
+    }
+  }
+
+ private:
+  bool IsAccelerating(double bemf_voltage, double voltage) {
+    if (bemf_voltage > 0) {
+      return voltage > bemf_voltage;
+    } else {
+      return voltage < bemf_voltage;
+    }
+  }
+};
 
 class Arm {
  public:
@@ -130,6 +178,8 @@
   void set_unprofiled_goal(double unprofiled_goal_shoulder,
                            double unprofiled_goal_wrist);
 
+  int controller_index() const { return loop_->controller_index(); }
+
   // Runs the controller and profile generator for a cycle.
   void Update(bool disabled);
 
@@ -196,7 +246,7 @@
 
   friend class testing::SuperstructureTest_DisabledGoalTest_Test;
   ::std::unique_ptr<
-      ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>> loop_;
+      ArmControlLoop> loop_;
 
   aos::util::TrapezoidProfile shoulder_profile_, wrist_profile_;
   ::frc971::zeroing::ZeroingEstimator shoulder_estimator_, wrist_estimator_;