diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index ea3aab8..c5d795c 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -154,6 +154,7 @@
     else:
       unaug_loop_writer.Write(argv[3], argv[4])
 
+    shooter = ShooterDeltaU()
     loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
     if argv[1][-3:] == '.cc':
       loop_writer.Write(argv[2], argv[1])
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 08138ff..c22b899 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -42,21 +42,47 @@
   U(0, 0) = voltage_ - old_voltage;
   //LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
   //LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
+  LOG(DEBUG, "Voltage sums up by %f\n", U(0, 0));
 
   last_voltage_ = voltage_;
 }
 
+void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
+                                             double known_position) {
+  LOG(INFO, "Setting calibration such that %f -> %f\n", encoder_val,
+      known_position);
+  LOG(INFO, "Position was %f\n", absolute_position());
+  double previous_offset = offset_;
+  offset_ = known_position - encoder_val;
+  double doffset = offset_ - previous_offset;
+  LOG(INFO, "Changing offset from %f to %f\n", previous_offset, offset_);
+  X_hat(0, 0) += doffset;
+  // Offset our measurements because the offset is baked into them.
+  Y_(0, 0) += doffset;
+  // Offset the goal so we don't move.
+  R(0, 0) += doffset;
+  LOG(INFO, "Validation: position is %f\n", absolute_position());
+}
+
 ShooterMotor::ShooterMotor(control_loops::ShooterGroup *my_shooter)
     : aos::control_loops::ControlLoop<control_loops::ShooterGroup>(my_shooter),
       shooter_(MakeShooterLoop()),
-      calibration_position_(0.0),
       state_(STATE_INITIALIZE),
       loading_problem_end_time_(0, 0),
+      load_timeout_(0, 0),
       shooter_brake_set_time_(0, 0),
+      unload_timeout_(0, 0),
       prepare_fire_end_time_(0, 0),
       shot_end_time_(0, 0),
-      cycles_not_moved_(0),
-      initial_loop_(true) {}
+      cycles_not_moved_(0) {}
+
+double ShooterMotor::PowerToPosition(double power) {
+  // LOG(WARNING, "power to position not correctly implemented\n");
+  const frc971::constants::Values &values = constants::GetValues();
+  double new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+                              values.shooter.upper_limit);
+  return new_pos;
+}
 
 // Positive is out, and positive power is out.
 void ShooterMotor::RunIteration(
@@ -73,12 +99,11 @@
     return;
   }
 
-  if (initial_loop_) {
-    // TODO(austin): If 'reset()', we are lost, start over.
-    initial_loop_ = false;
-    shooter_.SetPositionDirectly(position->position);
-  } else {
-    shooter_.SetPositionValues(position->position);
+  if (reset()) {
+    state_ = STATE_INITIALIZE;
+  }
+  if (position) {
+    shooter_.CorrectPosition(position->position);
   }
 
   // Disable the motors now so that all early returns will return with the
@@ -87,16 +112,8 @@
 
   const frc971::constants::Values &values = constants::GetValues();
 
-  double real_position = shooter_.position();
-  double adjusted_position = shooter_.position() - calibration_position_;
-
-  if (position) {
-    last_position_ = *position;
-    LOG(DEBUG,
-        "pos > real: %.2f adjusted: %.2f raw: %.2f calib: %.2f state= %d\n",
-        real_position, adjusted_position, position->position,
-        calibration_position_, state_);
-  }
+  double relative_position = shooter_.position();
+  double absolute_position = shooter_.absolute_position();
 
   // Don't even let the control loops run.
   bool shooter_loop_disable = false;
@@ -104,217 +121,357 @@
   // Adds voltage to take up slack in gears before shot.
   bool apply_some_voltage = false;
 
+  // TODO(austin): Observe not seeing the sensors when we should by moving the
+  // calibration offset correclty.
+  const bool disabled = !::aos::robot_state->enabled;
+
   switch (state_) {
     case STATE_INITIALIZE:
-      // Start off with the assumption that we are at the value
-      // futhest back given our sensors.
-      if (position && position->pusher_distal.current) {
-        //TODO(ben): use posedge
-        calibration_position_ =
-            position->position - values.shooter.pusher_distal.lower_angle;
-      } else if (position && position->pusher_proximal.current) {
-        //TODO(ben): use posedge
-        calibration_position_ =
-            position->position - values.shooter.pusher_proximal.lower_angle;
-      }
-
-      state_ = STATE_REQUEST_LOAD;
-
-      // Zero out initial goal.
-      shooter_.SetGoalPosition(real_position, 0.0);
       if (position) {
-        output->latch_piston = position->plunger;
+        // Reinitialize the internal filter state.
+        shooter_.InitializeState(position->position);
+        // TODO(austin): Test all of these initial states.
+
+        // Start off with the assumption that we are at the value
+        // futhest back given our sensors.
+        if (position->pusher_distal.current) {
+          shooter_.SetCalibration(position->position,
+                                  values.shooter.pusher_distal.lower_angle);
+        } else if (position->pusher_proximal.current) {
+          shooter_.SetCalibration(position->position,
+                                  values.shooter.pusher_proximal.lower_angle);
+        } else {
+          shooter_.SetCalibration(position->position,
+                                  values.shooter.upper_limit);
+        }
+
+        state_ = STATE_REQUEST_LOAD;
+
+        // Go to the current position.
+        shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+        // If the plunger is all the way back, we want to be latched.
+        latch_piston_ = position->plunger;
+        brake_piston_ = false;
       } else {
-        // We don't know what is going on so just close the latch to be safe.
-        output->latch_piston = true;
+        // If we can't start yet because we don't know where we are, set the
+        // latch and brake to their defaults.
+        latch_piston_ = true;
+        brake_piston_ = true;
       }
-      output->brake_piston = false;
       break;
     case STATE_REQUEST_LOAD:
-      if (position->plunger && position->latch) {
-        // Already latched.
-        state_ = STATE_PREPARE_SHOT;
-      } else if (position->pusher_distal.current || (adjusted_position) < 0) {
-        state_ = STATE_LOAD_BACKTRACK;
-        // TODO(ben): double check that rezero is the right thing to do here
-        if (position) {
-          calibration_position_ = position->position;
+      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;
+          latch_piston_ = true;
+        } else if (position->pusher_distal.current) {
+          // We started on the sensor, back up until we are found.
+          // If the plunger is all the way back and not latched, it won't be
+          // there for long.
+          state_ = STATE_LOAD_BACKTRACK;
+          latch_piston_ = false;
+        } else {
+          // Off the sensor, start loading.
+          Load();
+          latch_piston_ = false;
         }
-      } else {
-        state_ = STATE_LOAD;
       }
 
-      shooter_.SetGoalPosition(0.0, 0.0);
-      if (position && output) {
-        output->latch_piston = position->plunger;
-      }
-      if (output) {
-        output->brake_piston = false;
-      }
+      // Hold our current position.
+      shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+      brake_piston_ = false;
       break;
     case STATE_LOAD_BACKTRACK:
-      if (adjusted_position > values.shooter.pusher_distal.upper_angle + 0.01) {
+      // If we are here, then that means we started past the edge where we want
+      // to zero.  Move backwards until we don't see the sensor anymore.
+      // The plunger is contacting the pusher (or will be shortly).
+
+      // TODO(austin): Windup here and below!
+      if (!disabled) {
         shooter_.SetGoalPosition(
-            real_position - values.shooter.zeroing_speed * dt,
-            values.shooter.zeroing_speed);
-      } else {
-        state_ = STATE_LOAD;
+            shooter_.goal_position() - values.shooter.zeroing_speed * dt,
+            -values.shooter.zeroing_speed);
       }
 
-      if (output) output->latch_piston = false;
-      if (output) output->brake_piston = true;
+      if (position) {
+        if (!position->pusher_distal.current) {
+          Load();
+        }
+      }
+
+      latch_piston_ = false;
+      brake_piston_ = false;
       break;
     case STATE_LOAD:
-      if (position && position->pusher_proximal.current &&
-          !last_position_.pusher_proximal.current) {
-        //TODO(ben): use posedge
-        calibration_position_ =
-            position->position - values.shooter.pusher_proximal.upper_angle;
+      // If we are disabled right now, reset the timer.
+      if (disabled) {
+        Load();
+        // Latch defaults to true when disabled.  Leave it latched until we have
+        // useful sensor data.
+        latch_piston_ = true;
       }
-      if (position && position->pusher_distal.current &&
-          !last_position_.pusher_distal.current) {
-        //TODO(ben): use posedge
-        calibration_position_ =
-            position->position - values.shooter.pusher_distal.lower_angle;
-      }
+      // Go to 0, which should be the latch position, or trigger a hall effect
+      // on the way.  If we don't see edges where we are supposed to, the
+      // offset will be updated by code above.
+      shooter_.SetGoalPosition(0.0, 0.0);
 
-      shooter_.SetGoalPosition(calibration_position_, 0.0);
-      if (position && output) {
-        output->latch_piston = position->plunger;
-      }
+      if (position) {
+        // If we see a posedge on any of the hall effects,
+        if (position->pusher_proximal.posedge_count !=
+            last_proximal_posedge_count_) {
+          LOG(DEBUG, "Setting calibration using proximal sensor\n");
+          shooter_.SetCalibration(position->pusher_proximal.posedge_value,
+                                  values.shooter.pusher_proximal.upper_angle);
+        }
+        if (position->pusher_distal.posedge_count !=
+            last_distal_posedge_count_) {
+          LOG(DEBUG, "Setting calibration using distal sensor\n");
+          shooter_.SetCalibration(position->pusher_distal.posedge_value,
+                                  values.shooter.pusher_distal.upper_angle);
+        }
 
-      if (position->plunger && position->latch) {
-        state_ = STATE_PREPARE_SHOT;
-      } else if (position->plunger &&
-                 fabs(adjusted_position - PowerToPosition(goal->shot_power)) <
-                     0.05) {
-        state_ = STATE_LOADING_PROBLEM;
-        loading_problem_end_time_ =
-            Time::Now(Time::kDefaultClock) + Time::InSeconds(3.0);
+        // Latch if the plunger is far enough back to trigger the hall effect.
+        // This happens when the distal sensor is triggered.
+        latch_piston_ = position->pusher_distal.current;
+
+        // Check if we are latched and back.
+        if (position->plunger && position->latch) {
+          state_ = STATE_PREPARE_SHOT;
+        } else if (position->plunger &&
+                   ::std::abs(shooter_.absolute_position() -
+                              shooter_.goal_position()) < 0.001) {
+          // We are at the goal, but not latched.
+          state_ = STATE_LOADING_PROBLEM;
+          loading_problem_end_time_ = Time::Now() + Time::InSeconds(0.5);
+        }
       }
-      if (output) output->brake_piston = false;
+      if (load_timeout_ < Time::Now()) {
+        if (position) {
+          if (!position->pusher_distal.current ||
+              !position->pusher_proximal.current) {
+            state_ = STATE_ESTOP;
+          }
+        }
+      } else if (goal->unload_requested) {
+        Unload();
+      }
+      brake_piston_ = false;
       break;
     case STATE_LOADING_PROBLEM:
+      if (disabled) {
+        Load();
+      }
+      // We got to the goal, but the latch hasn't registered as down.  It might
+      // be stuck, or on it's way but not there yet.
       if (Time::Now() > loading_problem_end_time_) {
-        state_ = STATE_UNLOAD;
-      } else if (position->plunger && position->latch) {
+        // Timeout by unloading.
+        Unload();
+      } else if (position && position->plunger && position->latch) {
+        // If both trigger, we are latched.
         state_ = STATE_PREPARE_SHOT;
       }
-      shooter_.SetGoalPosition(calibration_position_, 0.0);
+      // Move a bit further back to help it trigger.
+      // If the latch is slow due to the air flowing through the tubes or
+      // inertia, but is otherwise free, this won't have much time to do
+      // anything and is safe.  Otherwise this gives us a bit more room to free
+      // up the latch.
+      shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
       LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
           position->plunger, position->latch);
 
-      if (output) output->latch_piston = true;
-      if (output) output->brake_piston = false;
+      latch_piston_ = true;
+      brake_piston_ = false;
       break;
     case STATE_PREPARE_SHOT:
+      // Move the shooter to the shot power set point and then lock the brake.
+      // TODO(austin): Timeout.  Low priority.
+
       shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
-      LOG(DEBUG, "PDIFF: adjusted_position: %.2f, pow: %.2f\n",
-          adjusted_position, PowerToPosition(goal->shot_power));
-      if (fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.05) {
+
+      LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
+          absolute_position, PowerToPosition(goal->shot_power));
+      // TODO(austin): Goal velocity too...
+      if (::std::abs(shooter_.absolute_position() -
+                     PowerToPosition(goal->shot_power)) < 0.001) {
+        // We are there, set the brake and move on.
+        latch_piston_ = true;
+        brake_piston_ = true;
+        shooter_brake_set_time_ = Time::Now() + Time::InSeconds(0.05);
         state_ = STATE_READY;
-        output->latch_piston = true;
-        output->brake_piston = true;
-        shooter_brake_set_time_ =
-            Time::Now(Time::kDefaultClock) + Time::InSeconds(0.03);
       } else {
-        output->latch_piston = true;
-        output->brake_piston = false;
+        latch_piston_ = true;
+        brake_piston_ = false;
+      }
+      if (goal->unload_requested) {
+        Unload();
       }
       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.
         shooter_loop_disable = true;
-        if (goal->unload_requested) {
-			printf("GHA\n");
-          state_ = STATE_UNLOAD;
-        } else if (fabs(adjusted_position - PowerToPosition(goal->shot_power)) >
-                   0.05) {
-			printf("GHB\n");
-          state_ = STATE_PREPARE_SHOT;
-        } else if (goal->shot_requested) {
-			printf("GHC\n");
-          state_ = STATE_REQUEST_FIRE;
+        LOG(DEBUG, "Brake is now set\n");
+        if (goal->shot_requested && !disabled) {
+          LOG(DEBUG, "Shooting now\n");
+          shooter_loop_disable = true;
+          prepare_fire_end_time_ =
+              Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
+          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);
       }
       shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
 
-      output->latch_piston = true;
-      output->brake_piston = true;
+      latch_piston_ = true;
+      brake_piston_ = true;
+
+      if (goal->unload_requested) {
+        Unload();
+      }
       break;
-    case STATE_REQUEST_FIRE:
+
+    case STATE_PREPARE_FIRE:
+      // Apply a bit of voltage to bias the gears for a little bit of time, and
+      // then fire.
       shooter_loop_disable = true;
-      if (position->plunger) {
+      if (disabled) {
+        // If we are disabled, reset the backlash bias timer.
         prepare_fire_end_time_ =
             Time::Now(Time::kDefaultClock) + Time::InMS(40.0);
-        apply_some_voltage = true;
-        state_ = STATE_PREPARE_FIRE;
-      } else {
-        state_ = STATE_REQUEST_LOAD;
+        break;
       }
-      break;
-    case STATE_PREPARE_FIRE:
-      shooter_loop_disable = true;
-      if (Time::Now(Time::kDefaultClock) < prepare_fire_end_time_) {
-        apply_some_voltage = true;
-      } else {
+      if (Time::Now() > prepare_fire_end_time_) {
+        cycles_not_moved_ = 0;
+        firing_starting_position_ = shooter_.absolute_position();
+        shot_end_time_ = Time::Now() + Time::InSeconds(1);
         state_ = STATE_FIRE;
-        cycles_not_moved_ = 0;
-        shot_end_time_ = Time::Now(Time::kDefaultClock) + Time::InMS(500);
+      } else {
+        apply_some_voltage = true;
       }
 
-      output->latch_piston = true;
-      output->brake_piston = true;
-
+      latch_piston_ = true;
+      brake_piston_ = true;
       break;
+
     case STATE_FIRE:
+      if (disabled) {
+        if (position) {
+          if (position->plunger) {
+            // If disabled and the plunger is still back there, reset the
+            // timeout.
+            shot_end_time_ = Time::Now() + Time::InSeconds(1);
+          }
+        }
+      }
       shooter_loop_disable = true;
-      //TODO(ben): need approamately equal
-      if (fabs(last_position_.position - adjusted_position) < 0.07) {
-        cycles_not_moved_++;
+      // Count the number of contiguous cycles during which we haven't moved.
+      if (::std::abs(last_position_.position - shooter_.absolute_position()) <
+          0.0005) {
+        ++cycles_not_moved_;
       } else {
         cycles_not_moved_ = 0;
       }
-      if ((adjusted_position < 0.10 && cycles_not_moved_ > 5) ||
-          Time::Now(Time::kDefaultClock) > shot_end_time_) {
+
+      // If we have moved any amount since the start and the shooter has now
+      // been still for a couple cycles, the shot finished.
+      // Also move on if it times out.
+      if ((::std::abs(firing_starting_position_ -
+                      shooter_.absolute_position()) > 0.0005 &&
+           cycles_not_moved_ > 3) ||
+          Time::Now() > shot_end_time_) {
         state_ = STATE_REQUEST_LOAD;
       }
-      output->latch_piston = true;
-      output->brake_piston = true;
+      latch_piston_ = false;
+      brake_piston_ = true;
       break;
     case STATE_UNLOAD:
+      // Reset the timeouts.
+      if (disabled) Unload();
+
+      // If it is latched and the plunger is back, move the pusher back to catch
+      // the plunger.
       if (position->plunger && position->latch) {
-        shooter_.SetGoalPosition(0.02, 0.0);
-        if (adjusted_position < 0.04) {
-          output->latch_piston = false;
+        // Pull back to 0, 0.
+        shooter_.SetGoalPosition(0.0, 0.0);
+        if (shooter_.absolute_position() < 0.005) {
+          // When we are close enough, 'fire'.
+          latch_piston_ = false;
+        } else {
+          latch_piston_ = true;
         }
       } else {
-        output->latch_piston = false;
+        // The plunger isn't all the way back, or it is and it is unlatched, so
+        // we can now unload.
+        shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+        latch_piston_ = false;
         state_ = STATE_UNLOAD_MOVE;
+        unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
       }
 
-      output->brake_piston = false;
+      if (Time::Now() > unload_timeout_) {
+        // We have been stuck trying to unload for way too long, give up and
+        // turn everything off.
+        state_ = STATE_ESTOP;
+      }
+
+      brake_piston_ = false;
       break;
-    case STATE_UNLOAD_MOVE:
-      if (adjusted_position > values.shooter.upper_limit - 0.03) {
-        shooter_.SetGoalPosition(real_position, 0.0);
+    case STATE_UNLOAD_MOVE: {
+      if (disabled) {
+        unload_timeout_ = Time::Now() + Time::InSeconds(2.0);
+        shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
+      }
+      // TODO(austin): Windup...
+
+      // Slowly move back until we hit the upper limit.
+      double goal_position =
+          shooter_.goal_position() + values.shooter.zeroing_speed * dt;
+      // If during this cycle, we would move past the limit, we are done
+      // unloading.
+      if (goal_position > values.shooter.upper_limit) {
+        shooter_.SetGoalPosition(values.shooter.upper_limit, 0.0);
+        // We don't want the loop fighting the spring when we are unloaded.
+        // Turn it off.
+        shooter_loop_disable = true;
         state_ = STATE_READY_UNLOAD;
       } else {
-        shooter_.SetGoalPosition(
-            real_position + values.shooter.zeroing_speed * dt,
-            values.shooter.zeroing_speed);
+        shooter_.SetGoalPosition(goal_position, values.shooter.zeroing_speed);
       }
 
-      output->latch_piston = false;
-      output->brake_piston = false;
-      break;
+      latch_piston_ = false;
+      brake_piston_ = false;
+    } break;
     case STATE_READY_UNLOAD:
-      if (!goal->unload_requested) {
+      if (goal->load_requested) {
         state_ = STATE_REQUEST_LOAD;
       }
+      // If we are ready to load again, 
+      shooter_loop_disable = true;
 
-      output->latch_piston = false;
-      output->brake_piston = false;
+      latch_piston_ = false;
+      brake_piston_ = false;
+      break;
+
+    case STATE_ESTOP:
+      // Totally lost, go to a safe state.
+      shooter_loop_disable = true;
+      latch_piston_ = true;
+      brake_piston_ = true;
       break;
   }
 
@@ -322,7 +479,8 @@
     shooter_.Update(true);
     if (output) output->voltage = 2.0;
   } else if (!shooter_loop_disable) {
-    LOG(DEBUG, "Running the loop, goal is %f\n", shooter_.R(0, 0));
+    LOG(DEBUG, "Running the loop, goal is %f, position is %f\n",
+        shooter_.goal_position(), shooter_.absolute_position());
     shooter_.Update(output == NULL);
     if (output) output->voltage = shooter_.voltage();
   } else {
@@ -330,8 +488,25 @@
     if (output) output->voltage = 0.0;
   }
 
+  if (output) {
+    output->latch_piston = latch_piston_;
+    output->brake_piston = brake_piston_;
+  }
+
   status->done =
-      ::std::fabs(adjusted_position - PowerToPosition(goal->shot_power)) < 0.004;
+      ::std::abs(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,
+        state_);
+  }
+  if (position) {
+    last_distal_posedge_count_ = position->pusher_distal.posedge_count;
+    last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
+  }
 }
 
 }  // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 89109dd..7e99bfc 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -36,9 +36,7 @@
         voltage_(0.0),
         last_voltage_(0.0),
         uncapped_voltage_(0.0),
-        offset_(0.0),
-        encoder_(0.0),
-        last_encoder_(0.0) {}
+        offset_(0.0) {}
 
   const static int kZeroingMaxVoltage = 5;
 
@@ -54,78 +52,46 @@
   // Zeros the accumulator.
   void ZeroPower() { voltage_ = 0.0; }
 
-  enum JointZeroingState {
-    // We don't know where the joint is at all.
-    UNKNOWN_POSITION,
-    // Ready for use during teleop.
-    CALIBRATED
-  };
-
-  void set_zeroing_state(JointZeroingState zeroing_state) {
-    zeroing_state_ = zeroing_state;
-  }
-
-  JointZeroingState zeroing_state() const { return zeroing_state_; }
-
   // Sets the calibration offset given the absolute angle and the corrisponding
   // encoder value.
-  void SetCalibration(double encoder_val, double known_position) {
-    offset_ = known_position - encoder_val;
-  }
+  void SetCalibration(double encoder_val, double known_position);
 
-  bool SetCalibrationOnEdge(const constants::Values::Shooter &shooter_values,
-                            JointZeroingState zeroing_state) {
-    double edge_encoder;
-    double known_position;
-    if (GetPositionOfEdge(shooter_values, &edge_encoder, &known_position)) {
-      LOG(INFO, "Calibration edge.\n");
-      SetCalibration(edge_encoder, known_position);
-      set_zeroing_state(zeroing_state);
-      return true;
-    }
-    return false;
-  }
+  double offset() const { return offset_; }
 
-  void SetPositionDirectly(double position) { X_hat(0, 0) = position; }
+  double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
 
-  void SetPositionValues(double position) {
+  void CorrectPosition(double position) {
     Eigen::Matrix<double, 1, 1> Y;
-    Y << position;
+    Y << position + offset_ - kPositionOffset;
     LOG(DEBUG, "Setting position to %f\n", position);
     Correct(Y);
   }
 
-  void SetGoalPosition(double desired_position, double desired_velocity) {
-    // austin said something about which matrix to set, but I didn't under
-    // very much of it
-    //some_matrix = {desired_position, desired_velocity};
-    LOG(DEBUG, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
-    R << desired_position, desired_velocity, 0;
+  double goal_position() const { return R(0, 0) + kPositionOffset; }
+  double goal_velocity() const { return R(1, 0); }
+  void InitializeState(double position) {
+    X_hat(0, 0) = position - kPositionOffset;
   }
 
-  double position() const { return X_hat(0, 0); }
-  double encoder() const { return encoder_; }
-  double last_encoder() const { return last_encoder_; }
+  void SetGoalPosition(double desired_position, double desired_velocity) {
+    LOG(DEBUG, "Goal position: %.2f Goal velocity: %.2f\n", desired_position, desired_velocity);
 
-  // Returns true if an edge was detected in the last cycle and then sets the
-  // edge_position to the absolute position of the edge.
-  bool GetPositionOfEdge(const constants::Values::Shooter &shooter,
-                         double *edge_encoder, double *known_position);
+    R << desired_position - kPositionOffset, desired_velocity,
+        -A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
+            A(1, 1) / A(1, 2) * desired_velocity;
+  }
 
-#undef COUNT_SETTER_GETTER
+  double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
 
  private:
+  // The offset between what is '0' (0 rate on the spring) and the 0 (all the
+  // way cocked).
+  constexpr static double kPositionOffset = 0.305054 + 0.0254;
   // The accumulated voltage to apply to the motor.
   double voltage_;
   double last_voltage_;
   double uncapped_voltage_;
   double offset_;
-
-  double previous_position_;
-
-  JointZeroingState zeroing_state_;
-  double encoder_;
-  double last_encoder_;
 };
 
 class ShooterMotor
@@ -137,17 +103,7 @@
   // True if the goal was moved to avoid goal windup.
   //bool capped_goal() const { return shooter_.capped_goal(); }
 
-  double PowerToPosition(double power) {
-    //LOG(WARNING, "power to position not correctly implemented\n");
-    const frc971::constants::Values &values = constants::GetValues();
-    double new_pos =
-        (power > values.shooter.upper_limit) ? values.shooter.upper_limit
-                                             : (power < 0.0)
-            ? 0.0
-            : power;
-
-    return new_pos;
-  }
+  double PowerToPosition(double power);
 
   typedef enum {
     STATE_INITIALIZE = 0,
@@ -157,20 +113,17 @@
     STATE_LOADING_PROBLEM = 4,
     STATE_PREPARE_SHOT = 5,
     STATE_READY = 6,
-    STATE_REQUEST_FIRE = 7,
-    STATE_PREPARE_FIRE = 8,
-    STATE_FIRE = 9,
-    STATE_UNLOAD = 10,
-    STATE_UNLOAD_MOVE = 11,
-    STATE_READY_UNLOAD = 12
+    STATE_PREPARE_FIRE = 7,
+    STATE_FIRE = 8,
+    STATE_UNLOAD = 9,
+    STATE_UNLOAD_MOVE = 10,
+    STATE_READY_UNLOAD = 11,
+    STATE_ESTOP = 12
   } State;
 
-  State GetState() {return state_;}
-
-  double GetPosition() { return shooter_.position() - calibration_position_; }
+  State state() { return state_; }
 
  protected:
-
   virtual void RunIteration(
       const ShooterGroup::Goal *goal,
       const control_loops::ShooterGroup::Position *position,
@@ -181,21 +134,35 @@
   friend class testing::ShooterTest_NoWindupPositive_Test;
   friend class testing::ShooterTest_NoWindupNegative_Test;
 
+  // Enter state STATE_UNLOAD
+  void Unload() {
+    state_ = STATE_UNLOAD;
+    unload_timeout_ = Time::Now() + Time::InSeconds(1);
+  }
+  // Enter state STATE_LOAD
+  void Load() {
+    state_ = STATE_LOAD;
+    load_timeout_ = Time::Now() + Time::InSeconds(1);
+  }
+
   control_loops::ShooterGroup::Position last_position_;
 
   ZeroedStateFeedbackLoop shooter_;
 
-  // position need to zero
-  double calibration_position_;
-
   // state machine state
   State state_;
 
   // time to giving up on loading problem
   Time loading_problem_end_time_;
 
+  // The end time when loading for it to timeout.
+  Time load_timeout_;
+
   // wait for brake to set
   Time shooter_brake_set_time_;
+  
+  // The timeout for unloading.
+  Time unload_timeout_;
 
   // we are attempting to take up some of the backlash
   // in the gears before the plunger hits
@@ -207,8 +174,13 @@
   // track cycles that we are stuck to detect errors
   int cycles_not_moved_;
 
-  // setup on the intial loop may involve shortcuts
-  bool initial_loop_;
+  double firing_starting_position_;
+
+  // True if the latch should be engaged and the brake should be engaged.
+  bool latch_piston_;
+  bool brake_piston_;
+  int32_t last_distal_posedge_count_;
+  int32_t last_proximal_posedge_count_;
 
   DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
 };
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 830ab84..6310320 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -19,10 +19,14 @@
     // Shoots as soon as this is true.
     bool shot_requested;
     bool unload_requested;
+    bool load_requested;
   };
 
   // Back is when the springs are all the way stretched.
   message Position {
+    // In meters, out is positive.
+    double position;
+
     // If the latch piston is fired and this hall effect has been triggered, the
     // plunger is all the way back and latched.
     bool plunger;
@@ -32,9 +36,6 @@
     PosedgeOnlyCountedHallEffectStruct pusher_proximal;
     // Triggers when the latch engages.
     bool latch;
-
-    // In meters, out is positive.
-    double position;
   };
   message Status {
     // Whether it's ready to shoot right now.
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index cad47af..2ea3afe 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -43,11 +43,15 @@
     Reinitialize(initial_position);
   }
 
+  // The difference between the position with 0 at the back, and the position
+  // with 0 measured where the spring has 0 force.
+  constexpr static double kPositionOffset = 0.3;
+
   void Reinitialize(double initial_position) {
     LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
     StateFeedbackPlant<2, 1, 1> *plant = shooter_plant_.get();
     initial_position_ = initial_position;
-    plant->X(0, 0) = initial_position_;
+    plant->X(0, 0) = initial_position_ - kPositionOffset;
     plant->X(1, 0) = 0.0;
     plant->Y = plant->C() * plant->X;
     last_voltage_ = 0.0;
@@ -56,7 +60,9 @@
   }
 
   // Returns the absolute angle of the wrist.
-  double GetAbsolutePosition() const { return shooter_plant_->Y(0, 0); }
+  double GetAbsolutePosition() const {
+    return shooter_plant_->Y(0, 0) + kPositionOffset;
+  }
 
   // Returns the adjusted angle of the wrist.
   double GetPosition() const {
@@ -72,18 +78,18 @@
   // (encoder, hall effect).
   void SetPhysicalSensors(control_loops::ShooterGroup::Position *position) {
     const frc971::constants::Values &values = constants::GetValues();
-    position->position = GetAbsolutePosition();
+    position->position = GetPosition();
 
-    LOG(DEBUG, "Physical shooter at {%f}\n", position->position);
+    LOG(DEBUG, "Physical shooter at {%f}\n", GetAbsolutePosition());
 
     // Signal that the hall effect sensor has been triggered if it is within
     // the correct range.
     position->plunger =
-        CheckRange(position->position, values.shooter.plunger_back);
+        CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
     position->pusher_distal.current =
-        CheckRange(position->position, values.shooter.pusher_distal);
+        CheckRange(GetAbsolutePosition(), values.shooter.pusher_distal);
     position->pusher_proximal.current =
-        CheckRange(position->position, values.shooter.pusher_proximal);
+        CheckRange(GetAbsolutePosition(), values.shooter.pusher_proximal);
   }
 
   void UpdateEffectEdge(
@@ -91,11 +97,18 @@
       const PosedgeOnlyCountedHallEffectStruct &last_sensor,
       const constants::Values::AnglePair &limits,
       const control_loops::ShooterGroup::Position &last_position) {
+    sensor->posedge_count = last_sensor.posedge_count;
+    sensor->negedge_count = last_sensor.negedge_count;
+
+    sensor->posedge_value = last_sensor.posedge_value;
+
     if (sensor->current && !last_sensor.current) {
       ++sensor->posedge_count;
-      if (last_position.position < limits.lower_angle) {
+      if (last_position.position + initial_position_ < limits.lower_angle) {
+        LOG(DEBUG, "Posedge value on lower edge of sensor, count is now %d\n", sensor->posedge_count);
         sensor->posedge_value = limits.lower_angle - initial_position_;
       } else {
+        LOG(DEBUG, "Posedge value on upper edge of sensor, count is now %d\n", sensor->posedge_count);
         sensor->posedge_value = limits.upper_angle - initial_position_;
       }
     }
@@ -112,47 +125,17 @@
 
     SetPhysicalSensors(position.get());
 
-    // Handle latch hall effect
-    if (!latch_piston_state_ && latch_delay_count_ > 0) {
-      LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
-      if (latch_delay_count_ == 1) {
-        latch_piston_state_ = true;
-        position->latch = true;
-      }
-      latch_delay_count_--;
-    } else if (latch_piston_state_ && latch_delay_count_ < 0) {
-      LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
-      if (latch_delay_count_ == -1) {
-        latch_piston_state_ = false;
-        position->latch = false;
-      }
-      latch_delay_count_++;
-    }
-
-    // Handle brake internal state
-    if (!brake_piston_state_ && brake_delay_count_ > 0) {
-      if (brake_delay_count_ == 1) {
-        brake_piston_state_ = true;
-      }
-      brake_delay_count_--;
-    } else if (brake_piston_state_ && brake_delay_count_ < 0) {
-      if (brake_delay_count_ == -1) {
-        brake_piston_state_ = false;
-      }
-      brake_delay_count_++;
-    }
+    position->latch = latch_piston_state_;
 
     // Handle pusher distal hall effect
-    UpdateEffectEdge(
-        &position->pusher_distal, last_position_message_.pusher_distal,
-        values.shooter.pusher_distal, last_position_message_);
-    LOG(INFO, "seteffect: pusher distal: %d\n", position->plunger);
+    UpdateEffectEdge(&position->pusher_distal,
+                     last_position_message_.pusher_distal,
+                     values.shooter.pusher_distal, last_position_message_);
 
     // Handle pusher proximal hall effect
-    UpdateEffectEdge(
-        &position->pusher_proximal, last_position_message_.pusher_proximal,
-        values.shooter.pusher_proximal, last_position_message_);
-    LOG(INFO, "seteffect: pusher proximal: %d\n", position->plunger);
+    UpdateEffectEdge(&position->pusher_proximal,
+                     last_position_message_.pusher_proximal,
+                     values.shooter.pusher_proximal, last_position_message_);
 
     last_position_message_ = *position;
     position.Send();
@@ -160,10 +143,8 @@
 
   // Simulates the claw moving for one timestep.
   void Simulate() {
-    last_plant_position_ = shooter_plant_->Y(0, 0);
+    last_plant_position_ = GetAbsolutePosition();
     EXPECT_TRUE(shooter_queue_group_.output.FetchLatest());
-    shooter_plant_->U << last_voltage_;
-    shooter_plant_->Update();
     if (shooter_queue_group_.output->latch_piston && !latch_piston_state_ &&
         latch_delay_count_ == 0) {
       latch_delay_count_ = 6;
@@ -180,11 +161,55 @@
       brake_delay_count_ = -5;
     }
 
-    EXPECT_GE(constants::GetValues().shooter.upper_limit,
-              shooter_plant_->Y(0, 0));
-    // we okay within a millimeter
-    EXPECT_LE(constants::GetValues().shooter.lower_limit - 0.001,
-              shooter_plant_->Y(0, 0));
+    // Handle brake internal state
+    if (!brake_piston_state_ && brake_delay_count_ > 0) {
+      if (brake_delay_count_ == 1) {
+        brake_piston_state_ = true;
+      }
+      brake_delay_count_--;
+    } else if (brake_piston_state_ && brake_delay_count_ < 0) {
+      if (brake_delay_count_ == -1) {
+        brake_piston_state_ = false;
+      }
+      brake_delay_count_++;
+    }
+
+    if (brake_piston_state_) {
+      shooter_plant_->U << 0.0;
+      shooter_plant_->X(1, 0) = 0.0;
+      shooter_plant_->Y = shooter_plant_->C() * shooter_plant_->X +
+                          shooter_plant_->D() * shooter_plant_->U;
+    } else {
+      shooter_plant_->U << last_voltage_;
+      shooter_plant_->Update();
+    }
+
+    // Handle latch hall effect
+    if (!latch_piston_state_ && latch_delay_count_ > 0) {
+      LOG(DEBUG, "latching simulation: %dp\n", latch_delay_count_);
+      if (latch_delay_count_ == 1) {
+        latch_piston_state_ = true;
+        EXPECT_GE(constants::GetValues().shooter.latch_max_safe_position,
+                  GetAbsolutePosition());
+      }
+      latch_delay_count_--;
+    } else if (latch_piston_state_ && latch_delay_count_ < 0) {
+      LOG(DEBUG, "latching simulation: %dn\n", latch_delay_count_);
+      if (latch_delay_count_ == -1) {
+        latch_piston_state_ = false;
+        EXPECT_TRUE(brake_piston_state_)
+            << ": Must have the brake set when releasing the latch.";
+        // TODO(austin): The brake should be set for a number of cycles after
+        // this as well.
+      }
+      latch_delay_count_++;
+    }
+
+    EXPECT_GE(constants::GetValues().shooter.upper_hard_limit,
+              GetAbsolutePosition());
+    EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
+              GetAbsolutePosition());
+
     last_voltage_ = shooter_queue_group_.output->voltage;
     ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
   }
@@ -232,7 +257,7 @@
             ".frc971.control_loops.shooter_queue_group.output",
             ".frc971.control_loops.shooter_queue_group.status"),
         shooter_motor_(&shooter_queue_group_),
-        shooter_motor_plant_(0.5) {
+        shooter_motor_plant_(0.2) {
     // Flush the robot state queue so we can use clean shared memory for this
     // test.
     ::aos::robot_state.Clear();
@@ -271,20 +296,20 @@
 TEST_F(ShooterTest, PowerConversion) {
   // test a couple of values return the right thing
   EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
-  EXPECT_EQ(0.475, shooter_motor_.PowerToPosition(0.475));
+  EXPECT_EQ(0.175, shooter_motor_.PowerToPosition(0.175));
 
   const frc971::constants::Values &values = constants::GetValues();
   // value too large should get max
   EXPECT_EQ(values.shooter.upper_limit,
             shooter_motor_.PowerToPosition(505050.99));
   // negative values should zero
-  EXPECT_EQ(0.0, shooter_motor_.PowerToPosition(-123.4));
+  EXPECT_EQ(values.shooter.lower_limit, shooter_motor_.PowerToPosition(-123.4));
 }
 
 // Tests that the wrist zeros correctly and goes to a position.
 TEST_F(ShooterTest, GoesToValue) {
   shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.21).Send();
-  for (int i = 0; i < 100; ++i) {
+  for (int i = 0; i < 200; ++i) {
     shooter_motor_plant_.SendPositionMessage();
     shooter_motor_.Iterate();
     shooter_motor_plant_.Simulate();
@@ -293,9 +318,8 @@
   // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
   double pos = shooter_motor_plant_.GetAbsolutePosition();
   EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
-  EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
+  EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
 }
-
 // Tests that the wrist zeros correctly and goes to a position.
 TEST_F(ShooterTest, Fire) {
   shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.021).Send();
@@ -305,10 +329,9 @@
     shooter_motor_plant_.Simulate();
     SendDSPacket(true);
   }
-  EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
+  EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
   shooter_queue_group_.goal.MakeWithBuilder().shot_requested(true).Send();
 
-  bool hit_requestfire = false;
   bool hit_preparefire = false;
   bool hit_fire = false;
   for (int i = 0; i < 100; ++i) {
@@ -316,26 +339,24 @@
     shooter_motor_.Iterate();
     shooter_motor_plant_.Simulate();
     SendDSPacket(true);
-    printf("MOTORSTATE = %d\n", shooter_motor_.GetState());
-    if (shooter_motor_.GetState() == ShooterMotor::STATE_REQUEST_FIRE) {
-      hit_requestfire = true;
-    }
-    if (shooter_motor_.GetState() == ShooterMotor::STATE_PREPARE_FIRE) {
+    LOG(DEBUG, "MOTORSTATE = %d\n", shooter_motor_.state());
+    if (shooter_motor_.state() == ShooterMotor::STATE_PREPARE_FIRE) {
       hit_preparefire = true;
     }
-    if (shooter_motor_.GetState() == ShooterMotor::STATE_FIRE) {
+    if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
       hit_fire = true;
     }
   }
 
   double pos = shooter_motor_plant_.GetAbsolutePosition();
   EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
-  EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.GetState());
-  EXPECT_TRUE(hit_requestfire);
+  EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
   EXPECT_TRUE(hit_preparefire);
   EXPECT_TRUE(hit_fire);
 }
 
+// TODO(austin): Test all the timeouts...
+
 }  // namespace testing
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
index 1b0e13c..de33fa7 100755
--- a/frc971/control_loops/shooter/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -9,7 +9,7 @@
 
 StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
   Eigen::Matrix<double, 3, 3> A;
-  A << 1.0, 0.00988697090637, 0.000120553991591, 0.0, 0.977479674375, 0.0240196135246, 0.0, 0.0, 1.0;
+  A << 0.998324052598, 0.0007783475087, 0.000278701304898, -0.181614418697, -0.000138907346386, 0.0302015298419, 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 3, 1> B;
   B << 0.0, 0.0, 1.0;
   Eigen::Matrix<double, 1, 3> C;
@@ -25,9 +25,9 @@
 
 StateFeedbackController<3, 1, 1> MakeShooterController() {
   Eigen::Matrix<double, 3, 1> L;
-  L << 1.97747967438, 101.419434198, 375.761249895;
+  L << 0.998185145251, 11.8167175789, 298.617717297;
   Eigen::Matrix<double, 1, 3> K;
-  K << 243.5509628, 18.9166116502, 1.27747967438;
+  K << 162.58140285, 6.68264124674, 0.198185145251;
   return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
index d395200..fce7579 100755
--- a/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter/unaugmented_shooter_motor_plant.cc
@@ -9,9 +9,9 @@
 
 StateFeedbackPlantCoefficients<2, 1, 1> MakeRawShooterPlantCoefficients() {
   Eigen::Matrix<double, 2, 2> A;
-  A << 1.0, 0.00988697090637, 0.0, 0.977479674375;
+  A << 0.998324052598, 0.0007783475087, -0.181614418697, -0.000138907346386;
   Eigen::Matrix<double, 2, 1> B;
-  B << 0.000120553991591, 0.0240196135246;
+  B << 0.000278701304898, 0.0302015298419;
   Eigen::Matrix<double, 1, 2> C;
   C << 1, 0;
   Eigen::Matrix<double, 1, 1> D;
@@ -25,9 +25,9 @@
 
 StateFeedbackController<2, 1, 1> MakeRawShooterController() {
   Eigen::Matrix<double, 2, 1> L;
-  L << 1.87747967438, 87.0117404538;
+  L << 0.898185145251, 3.04818975205;
   Eigen::Matrix<double, 1, 2> K;
-  K << 343.469306513, 26.4814035344;
+  K << 994.822639009, -5.92927654062;
   return StateFeedbackController<2, 1, 1>(L, K, MakeRawShooterPlantCoefficients());
 }
 
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0384747..c58d7e5 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -314,6 +314,7 @@
   void Update(bool stop_motors) {
     if (stop_motors) {
       U.setZero();
+      U_uncapped.setZero();
     } else {
       U = U_uncapped = K() * (R - X_hat);
       CapU();