Shooter now unloads propperly.  Need to figure out why it is overshooting when the python isn't.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index faae2b0..fb9df74 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -29,16 +29,20 @@
   // Make sure that reality and the observer can't get too far off.  There is a
   // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
   // against last cycle's voltage.
-  if (X_hat(2, 0) > last_voltage_ + 2.0) {
-    voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
-  } else if (X_hat(2, 0) < last_voltage_ - 2.0) {
-    voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
+  if (X_hat(2, 0) > last_voltage_ + 4.0) {
+    voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
+    LOG(INFO, "Capping due to runawway\n");
+  } else if (X_hat(2, 0) < last_voltage_ - 4.0) {
+    voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
+    LOG(INFO, "Capping due to runawway\n");
   }
 
   voltage_ = std::min(limit, voltage_);
   voltage_ = std::max(-limit, voltage_);
   U(0, 0) = voltage_ - old_voltage;
 
+  LOG(INFO, "X_hat is %f, applied is %f\n", X_hat(2, 0), voltage_);
+
   last_voltage_ = voltage_;
 }
 
@@ -107,9 +111,6 @@
 
   const frc971::constants::Values &values = constants::GetValues();
 
-  double relative_position = shooter_.position();
-  double absolute_position = shooter_.absolute_position();
-
   // Don't even let the control loops run.
   bool shooter_loop_disable = false;
 
@@ -122,7 +123,6 @@
 
   switch (state_) {
     case STATE_INITIALIZE:
-      LOG(DEBUG, "Initializing\n");
       if (position) {
         // Reinitialize the internal filter state.
         shooter_.InitializeState(position->position);
@@ -157,8 +157,6 @@
       break;
     case STATE_REQUEST_LOAD:
       if (position) {
-        // Need to go forward a little if we are starting with the
-        // back_distal_sensor triggered
         if (position->plunger && position->latch) {
           // The plunger is back and we are latched, get ready to shoot.
           state_ = STATE_PREPARE_SHOT;
@@ -288,10 +286,11 @@
       shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
 
       LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
-          absolute_position, PowerToPosition(goal->shot_power));
-      // TODO(austin): Goal velocity too...
+          shooter_.absolute_position(), PowerToPosition(goal->shot_power));
       if (::std::abs(shooter_.absolute_position() -
-                     PowerToPosition(goal->shot_power)) < 0.001) {
+                     PowerToPosition(goal->shot_power)) +
+              ::std::abs(shooter_.absolute_velocity()) <
+          0.001) {
         // We are there, set the brake and move on.
         latch_piston_ = true;
         brake_piston_ = true;
@@ -307,9 +306,19 @@
       break;
     case STATE_READY:
       LOG(DEBUG, "In ready\n");
-      // Wait until the brake is set, and a shot is requested or the shot power is changed.
-      if (Time::Now() > shooter_brake_set_time_) {
-        // We have waited long enough for the brake to set, turn the shooter control loop off.
+      // Wait until the brake is set, and a shot is requested or the shot power
+      // is changed.
+      if (::std::abs(shooter_.absolute_position() -
+                     PowerToPosition(goal->shot_power)) > 0.002) {
+        // TODO(austin): Add a state to release the brake.
+
+        // TODO(austin): Do we want to set the brake here or after shooting?
+        // Depends on air usage.
+        LOG(DEBUG, "Preparing shot again.\n");
+        state_ = STATE_PREPARE_SHOT;
+      } else if (Time::Now() > shooter_brake_set_time_) {
+        // We have waited long enough for the brake to set, turn the shooter
+        // control loop off.
         shooter_loop_disable = true;
         LOG(DEBUG, "Brake is now set\n");
         if (goal->shot_requested && !disabled) {
@@ -320,14 +329,6 @@
           apply_some_voltage = true;
           state_ = STATE_PREPARE_FIRE;
         }
-      } else if (::std::abs(shooter_.absolute_position() -
-                            PowerToPosition(goal->shot_power)) > 0.001) {
-        // TODO(austin): If the goal has changed, go back to prepare shot, not if we are off.
-        // TODO(austin): Add a state to release the brake.
-
-        // TODO(austin): Do we want to set the brake here or after shooting?
-        LOG(DEBUG, "Preparing shot again.\n");
-        state_ = STATE_PREPARE_SHOT;
       } else {
         LOG(DEBUG, "Nothing %d %d\n", goal->shot_requested, !disabled);
       }
@@ -491,13 +492,13 @@
   }
 
   status->done =
-      ::std::abs(absolute_position - PowerToPosition(goal->shot_power)) < 0.004;
+      ::std::abs(shooter_.absolute_position() - PowerToPosition(goal->shot_power)) < 0.004;
 
   if (position) {
     last_position_ = *position;
     LOG(DEBUG,
-        "pos > real: %.2f adjusted: %.2f raw: %.2f state= %d\n",
-        relative_position, absolute_position, position->position,
+        "pos > absolute: %f velocity: %f state= %d\n",
+        shooter_.absolute_position(), shooter_.absolute_velocity(),
         state_);
   }
   if (position) {