Worked through the shooter loop and fixed a bunch of bugs.  Switched the internal state in the loop to use the 0 point of the spring, fixed problems with the shooter, and pushed calibratoin into the loop.  Lots better now.
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