diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 9e12b6b..70740d5 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -24,10 +24,11 @@
 
 
 class Arm(control_loop.ControlLoop):
-  def __init__(self, name="Arm"):
+  def __init__(self, name="Arm", J=None):
     super(Arm, self).__init__(name=name)
-    self._shoulder = Shoulder(name=name)
+    self._shoulder = Shoulder(name=name, J=J)
     self._shooter = Wrist(name=name)
+    self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
 
     # Do a coordinate transformation.
     # X_shooter_grounded = X_shooter + X_shoulder
@@ -94,7 +95,10 @@
     self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
 
     glog.debug('Shoulder K')
-    glog.debug(self._shoulder.K)
+    glog.debug(repr(self._shoulder.K))
+    glog.debug('Poles are %s',
+        repr(numpy.linalg.eig(self._shoulder.A -
+                              self._shoulder.B * self._shoulder.K)[0]))
 
     # Compute controller gains.
     # self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
@@ -135,8 +139,8 @@
 
 
 class IntegralArm(Arm):
-  def __init__(self, name="IntegralArm"):
-    super(IntegralArm, self).__init__(name=name)
+  def __init__(self, name="IntegralArm", J=None):
+    super(IntegralArm, self).__init__(name=name, J=J)
 
     self.A_continuous_unaugmented = self.A_continuous
     self.B_continuous_unaugmented = self.B_continuous
@@ -154,11 +158,11 @@
 
     self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
 
-    q_pos_shoulder = 0.08
-    q_vel_shoulder = 4.00
-    q_voltage_shoulder = 3.0
+    q_pos_shoulder = 0.10
+    q_vel_shoulder = 0.005
+    q_voltage_shoulder = 3.5
     q_pos_shooter = 0.08
-    q_vel_shooter = 4.00
+    q_vel_shooter = 2.00
     q_voltage_shooter = 1.0
     self.Q = numpy.matrix(numpy.zeros((6, 6)))
     self.Q[0, 0] = q_pos_shoulder ** 2.0
@@ -236,7 +240,7 @@
     goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
 
     shoulder_profile = TrapezoidProfile(arm.dt)
-    shoulder_profile.set_maximum_acceleration(50.0)
+    shoulder_profile.set_maximum_acceleration(12.0)
     shoulder_profile.set_maximum_velocity(10.0)
     shoulder_profile.SetGoal(goal[0, 0])
     shooter_profile = TrapezoidProfile(arm.dt)
@@ -318,8 +322,6 @@
       self.u_shoulder.append(U[0, 0])
       self.u_shooter.append(U[1, 0])
 
-      glog.debug('Time: %f', self.t[-1])
-
       ff_U -= U_uncapped - U
       goal = controller.A * goal + controller.B * ff_U
 
@@ -378,7 +380,7 @@
   scenario_plotter = ScenarioPlotter()
 
   arm = Arm()
-  arm_controller = IntegralArm()
+  arm_controller = IntegralArm(name='AcceleratingIntegralArm', J=12)
   arm_observer = IntegralArm()
 
   # Test moving the shoulder with constant separation.
@@ -406,8 +408,12 @@
                                                  namespaces=namespaces)
     loop_writer.Write(argv[1], argv[2])
 
+    decelerating_arm_controller = IntegralArm(name='DeceleratingIntegralArm', J=5)
     integral_loop_writer = control_loop.ControlLoopWriter(
-        'IntegralArm', [arm_controller], namespaces=namespaces)
+        'IntegralArm', [arm_controller, decelerating_arm_controller],
+        namespaces=namespaces)
+    integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
+          arm_controller.shoulder_Kv))
     integral_loop_writer.Write(argv[3], argv[4])
 
   if FLAGS.plot:
diff --git a/y2016/control_loops/python/intake.py b/y2016/control_loops/python/intake.py
index 63cc0f4..da45bd1 100755
--- a/y2016/control_loops/python/intake.py
+++ b/y2016/control_loops/python/intake.py
@@ -42,7 +42,7 @@
 
     # Moment of inertia, measured in CAD.
     # Extra mass to compensate for friction is added on.
-    self.J = 0.34 + 0.1
+    self.J = 0.34 + 0.6
 
     # Control loop time step
     self.dt = 0.005
@@ -138,11 +138,12 @@
     self.C = numpy.matrix(numpy.zeros((1, 3)))
     self.C[0:1, 0:2] = self.C_unaugmented
 
-    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
 
     q_pos = 0.12
     q_vel = 2.00
-    q_voltage = 3.0
+    q_voltage = 4.0
     self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
                            [0.0, (q_vel ** 2.0), 0.0],
                            [0.0, 0.0, (q_voltage ** 2.0)]])
@@ -235,7 +236,10 @@
       self.v.append(intake.X[1, 0])
       self.a.append((self.v[-1] - last_v) / intake.dt)
 
-      intake.Update(U + 0.0)
+      offset = 0.0
+      if i > 100:
+        offset = 2.0
+      intake.Update(U + offset)
 
       observer_intake.PredictObserver(U)
 
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 07a67f6..6beac61 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -17,7 +17,7 @@
   pass
 
 class Shoulder(control_loop.ControlLoop):
-  def __init__(self, name="Shoulder", mass=None):
+  def __init__(self, name="Shoulder", J=None):
     super(Shoulder, self).__init__(name)
     # TODO(constants): Update all of these & retune poles.
     # Stall Torque in N m
@@ -39,7 +39,10 @@
     # Gear ratio
     self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (42.0 / 12.0)
 
-    self.J = 3.0
+    if J is None:
+      self.J = 10.0
+    else:
+      self.J = J
 
     # Control loop time step
     self.dt = 0.005
@@ -67,8 +70,8 @@
 
     controllability = controls.ctrb(self.A, self.B)
 
-    q_pos = 0.14
-    q_vel = 4.5
+    q_pos = 0.16
+    q_vel = 1.5
     self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
                            [0.0, (1.0 / (q_vel ** 2.0))]])
 
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 4d4f5f1..660e577 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -39,7 +39,7 @@
     # Gear ratio
     self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
 
-    self.J = 0.15
+    self.J = 0.20
 
     # Control loop time step
     self.dt = 0.005
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_;