diff --git a/frc971/control_loops/index.cc b/frc971/control_loops/index.cc
index 845f1c6..6d72fe2 100644
--- a/frc971/control_loops/index.cc
+++ b/frc971/control_loops/index.cc
@@ -23,37 +23,48 @@
       wrist_loop_(new StateFeedbackLoop<2, 1, 1>(MakeIndexLoop())),
       hopper_disc_count_(0),
       total_disc_count_(0),
+      safe_goal_(Goal::HOLD),
+      loader_goal_(LoaderGoal::READY),
+      loader_state_(LoaderState::READY),
       loader_up_(false),
       disc_clamped_(false),
       disc_ejected_(false),
       last_bottom_disc_detect_(false) {
 }
 
+/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
+/*static*/ const double IndexMotor::kIndexStartPosition = 0.2159;
+/*static*/ const double IndexMotor::kIndexFreeLength =
+      IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
+/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
+      kIndexStartPosition + kIndexFreeLength;
+/*static*/ const double IndexMotor::kReadyToLiftPosition =
+    kLoaderFreeStopPosition + 0.2921;
+/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
+/*static*/ const double IndexMotor::kGrabberStartPosition =
+    kReadyToLiftPosition - kGrabberLength;
+/*static*/ const double IndexMotor::kGrabberMovementVelocity = 0.5;
+/*static*/ const double IndexMotor::kLifterStopPosition =
+    kReadyToLiftPosition + 0.161925;
+/*static*/ const double IndexMotor::kLifterMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kEjectorStopPosition =
+    kLifterStopPosition + 0.01;
+/*static*/ const double IndexMotor::kEjectorMovementVelocity = 1.0;
+/*static*/ const double IndexMotor::kBottomDiscDetectStart = -0.08;
+/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.200025;
+
+// TODO(aschuh): Figure these out.
+/*static*/ const double IndexMotor::kTopDiscDetectStart = 18.0;
+/*static*/ const double IndexMotor::kTopDiscDetectStop = 19.0;
+
 const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
 const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
 
-bool IndexMotor::FetchConstants() {
-  if (!constants::horizontal_lower_limit(&horizontal_lower_limit_)) {
-    LOG(ERROR, "Failed to fetch the horizontal lower limit constant.\n");
-    return false;
-  }
-  if (!constants::horizontal_upper_limit(&horizontal_upper_limit_)) {
-    LOG(ERROR, "Failed to fetch the horizontal upper limit constant.\n");
-    return false;
-  }
-  if (!constants::horizontal_hall_effect_start_angle(
-          &horizontal_hall_effect_start_angle_)) {
-    LOG(ERROR, "Failed to fetch the horizontal start angle constant.\n");
-    return false;
-  }
-  if (!constants::horizontal_zeroing_speed(
-          &horizontal_zeroing_speed_)) {
-    LOG(ERROR, "Failed to fetch the horizontal zeroing speed constant.\n");
-    return false;
-  }
-
-  return true;
-}
+/*static*/ const int IndexMotor::kGrabbingDelay = 5;
+/*static*/ const int IndexMotor::kLiftingDelay = 20;
+/*static*/ const int IndexMotor::kShootingDelay = 5;
+/*static*/ const int IndexMotor::kLoweringDelay = 20;
 
 // Distance to move the indexer when grabbing a disc.
 const double kNextPosition = 10.0;
@@ -62,10 +73,16 @@
   return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
 }
 
-/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(const double angle) {
+/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(
+    const double angle) {
   return angle * (kDiscRadius + kRollerRadius);
 }
 
+/*static*/ double IndexMotor::ConvertDiscPositionToDiscAngle(
+    const double position) {
+  return position / (kDiscRadius + kRollerRadius);
+}
+
 /*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
   return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
 }
@@ -75,6 +92,53 @@
       ConvertIndexToDiscAngle(angle));
 }
 
+/*static*/ double IndexMotor::ConvertTransferToDiscPosition(
+    const double angle) {
+  const double gear_ratio =  (1 + (kDiscRadius * 2 + kTransferRollerRadius) /
+                              kTransferRollerRadius);
+  return angle / gear_ratio * (kDiscRadius + kTransferRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertDiscPositionToIndex(
+    const double position) {
+  return IndexMotor::ConvertDiscAngleToIndex(
+      ConvertDiscPositionToDiscAngle(position));
+}
+
+bool IndexMotor::MinDiscPosition(double *disc_position) {
+  bool found_start = false;
+  for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+    const Frisbee &frisbee = frisbees_[i];
+    if (!found_start) {
+      if (frisbee.has_position()) {
+        *disc_position = frisbee.position();
+        found_start = true;
+      }
+    } else {
+      *disc_position = ::std::min(frisbee.position(),
+                                  *disc_position);
+    }
+  }
+  return found_start;
+}
+
+bool IndexMotor::MaxDiscPosition(double *disc_position) {
+  bool found_start = false;
+  for (unsigned int i = 0; i < frisbees_.size(); ++i) {
+    const Frisbee &frisbee = frisbees_[i];
+    if (!found_start) {
+      if (frisbee.has_position()) {
+        *disc_position = frisbee.position();
+        found_start = true;
+      }
+    } else {
+      *disc_position = ::std::max(frisbee.position(),
+                                  *disc_position);
+    }
+  }
+  return found_start;
+}
+
 // Positive angle is towards the shooter, and positive power is towards the
 // shooter.
 void IndexMotor::RunIteration(
@@ -87,6 +151,7 @@
 
   // Disable the motors now so that all early returns will return with the
   // motors disabled.
+  double transfer_voltage = 0.0;
   if (output) {
     output->transfer_voltage = 0.0;
     output->index_voltage = 0.0;
@@ -94,32 +159,28 @@
 
   status->ready_to_intake = false;
 
-  // Cache the constants to avoid error handling down below.
-  if (!FetchConstants()) {
-    return;
-  }
-
+  // Compute a safe index position that we can use.
   if (position) {
     wrist_loop_->Y << position->index_position;
   }
   const double index_position = wrist_loop_->X_hat(0, 0);
 
+  // TODO(aschuh): Watch for top disc detect and update the frisbee
+  // position.
+
+  // TODO(aschuh): Horizontal and centering should be here as well...
+
+  // Bool to track if it is safe for the goal to change yet.
   bool safe_to_change_state_ = true;
   switch (safe_goal_) {
-    case HOLD:
+    case Goal::HOLD:
       // The goal should already be good, so sit tight with everything the same
       // as it was.
-      printf("HOLD Not implemented\n");
       break;
-    case READY_LOWER:
-      printf("READY_LOWER Not implemented\n");
-      break;
-    case INTAKE:
+    case Goal::READY_LOWER:
+    case Goal::INTAKE:
       {
         Time now = Time::Now();
-        if (hopper_disc_count_ < 4) {
-          output->transfer_voltage = 12.0;
-        }
         // Posedge of the disc entering the beam break.
         if (position) {
           if (position->bottom_disc_detect && !last_bottom_disc_detect_) {
@@ -128,6 +189,7 @@
             printf("Posedge of bottom disc %f\n",
                    transfer_frisbee_.bottom_posedge_time_.ToSeconds());
             ++hopper_disc_count_;
+            ++total_disc_count_;
           }
 
           // Disc exited the beam break now.
@@ -139,12 +201,12 @@
           }
 
           if (position->bottom_disc_detect) {
-            output->transfer_voltage = 12.0;
+            transfer_voltage = 12.0;
             // Must wait until the disc gets out before we can change state.
             safe_to_change_state_ = false;
 
-            // TODO(aschuh): A disc on the way through needs to start moving the
-            // indexer if it isn't already moving.  Maybe?
+            // TODO(aschuh): A disc on the way through needs to start moving
+            // the indexer if it isn't already moving.  Maybe?
 
             Time elapsed_posedge_time = now -
                 transfer_frisbee_.bottom_posedge_time_;
@@ -155,12 +217,13 @@
             }
           }
 
+          // Check all non-indexed discs and see if they should be indexed.
           for (Frisbee &frisbee : frisbees_) {
             if (!frisbee.has_been_indexed_) {
-              output->transfer_voltage = 12.0;
-              Time elapsed_posedge_time = now -
-                  frisbee.bottom_posedge_time_;
-              if (elapsed_posedge_time >= Time::InSeconds(0.07)) {
+              transfer_voltage = 12.0;
+              Time elapsed_negedge_time = now -
+                  frisbee.bottom_negedge_time_;
+              if (elapsed_negedge_time >= Time::InSeconds(0.005)) {
                 // Should have just engaged.
                 // Save the indexer position, and the time.
 
@@ -170,44 +233,236 @@
                 printf("Grabbed on the index now at %f\n", index_position);
                 frisbee.has_been_indexed_ = true;
                 frisbee.index_start_position_ = index_position;
-                frisbee.index_start_time_ = now;
               }
             }
             if (!frisbee.has_been_indexed_) {
-              // Discs must all be indexed before it is safe to stop indexing.
+              // All discs must be indexed before it is safe to stop indexing.
               safe_to_change_state_ = false;
             }
           }
 
-          double new_index_position = wrist_loop_->R(0, 0);
+          // Figure out where the indexer should be to move the discs down to
+          // the right position.
+          double max_disc_position;
+          if (MaxDiscPosition(&max_disc_position)) {
+            printf("There is a disc down here!\n");
+            // TODO(aschuh): Figure out what to do if grabbing the next one
+            // would cause things to jam into the loader.
+            // Say we aren't ready any more.  Undefined behavior will result if
+            // that isn't observed.
+            double bottom_disc_position =
+                max_disc_position + ConvertDiscAngleToIndex(M_PI);
+            wrist_loop_->R << bottom_disc_position, 0.0;
 
-          // TODO(aschuh): As we loop through, assess the state of the indexer
-          // and figure if the bottom disc is in a place such that we can
-          // intake without filling the hopper early.
-          // status->ready_to_intake = false;
-
-          for (Frisbee &frisbee : frisbees_) {
-            if (frisbee.has_been_indexed_) {
-              // We want to store it pi from where the disc was grabbed
-              // (for now).
-              new_index_position = ::std::max(
-                  new_index_position,
-                  (frisbee.index_start_position_ +
-                   ConvertDiscAngleToIndex(M_PI)));
-              // TODO(aschuh): We should be able to pick the M_PI knowing if
-              // the next disc is coming in hot or not.
+            // Verify that we are close enough to the goal so that we should be
+            // fine accepting the next disc.
+            double disc_error_meters = ConvertIndexToDiscPosition(
+                wrist_loop_->X_hat(0, 0) - bottom_disc_position);
+            // We are ready for the next disc if the first one is in the first
+            // half circle of the indexer.  It will take time for the disc to
+            // come into the indexer, so we will be able to move it out of the
+            // way in time.
+            // This choice also makes sure that we don't claim that we aren't
+            // ready between full speed intaking.
+            if (-ConvertDiscAngleToIndex(M_PI) < disc_error_meters &&
+                disc_error_meters < 0.04) {
+              // We are only ready if we aren't being asked to change state or
+              // are full.
+              status->ready_to_intake =
+                  (safe_goal_ == goal_enum) && hopper_disc_count_ < 4;
+            } else {
+              status->ready_to_intake = false;
             }
+          } else {
+            // No discs!  We are always ready for more if we aren't being
+            // asked to change state.
+            status->ready_to_intake = (safe_goal_ == goal_enum);
           }
-          wrist_loop_->R << new_index_position, 0.0;
+
+          // Turn on the transfer roller if we are ready.
+          if (status->ready_to_intake && hopper_disc_count_ < 4 &&
+              safe_goal_ == Goal::INTAKE) {
+            transfer_voltage = 12.0;
+          }
         }
-        printf("INTAKE Not implemented\n");
+        printf("INTAKE\n");
       }
       break;
-    case READY_SHOOTER:
-      printf("READY_SHOOTER Not implemented\n");
+    case Goal::READY_SHOOTER:
+    case Goal::SHOOT:
+      // Check if we have any discs to shoot or load and handle them.
+      double min_disc_position;
+      if (MinDiscPosition(&min_disc_position)) {
+        const double ready_disc_position =
+            min_disc_position + ConvertDiscPositionToIndex(kIndexFreeLength) -
+            ConvertDiscAngleToIndex(M_PI / 6.0);
+
+        const double grabbed_disc_position =
+            min_disc_position +
+            ConvertDiscPositionToIndex(kReadyToLiftPosition -
+                                       kIndexStartPosition + 0.03);
+
+        // Check the state of the loader FSM.
+        // If it is ready to load discs, position the disc so that it is ready
+        // to be grabbed.
+        // If it isn't ready, there is a disc in there.  It needs to finish it's
+        // cycle first.
+        if (loader_state_ != LoaderState::READY) {
+          // We already have a disc in the loader.
+          // Stage the discs back a bit.
+          wrist_loop_->R << ready_disc_position, 0.0;
+
+          // Must wait until it has been grabbed to continue.
+          if (loader_state_ == LoaderState::GRABBING) {
+            safe_to_change_state_ = false;
+          }
+        } else {
+          // No disc up top right now.
+          wrist_loop_->R << grabbed_disc_position, 0.0;
+
+          // See if the disc has gotten pretty far up yet.
+          if (wrist_loop_->X_hat(0, 0) > ready_disc_position) {
+            // Point of no return.  We are committing to grabbing it now.
+            safe_to_change_state_ = false;
+            const double robust_grabbed_disc_position =
+                (grabbed_disc_position -
+                 ConvertDiscPositionToIndex(kGrabberLength));
+
+            // If close, start grabbing and/or shooting.
+            if (wrist_loop_->X_hat(0, 0) > robust_grabbed_disc_position) {
+              // Start the state machine.
+              if (safe_goal_ == Goal::SHOOT) {
+                loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+              } else {
+                loader_goal_ = LoaderGoal::GRAB;
+              }
+              // This frisbee is now gone.  Take it out of the queue.
+              frisbees_.pop_back();
+              --hopper_disc_count_;
+            }
+          }
+        }
+      }
+
+      printf("READY_SHOOTER or SHOOT\n");
       break;
-    case SHOOT:
-      printf("SHOOT Not implemented\n");
+  }
+
+  // The only way out of the loader is to shoot the disc.  The FSM can only go
+  // forwards.
+  switch (loader_state_) {
+    case LoaderState::READY:
+      printf("Loader READY\n");
+      // Open and down, ready to accept a disc.
+      loader_up_ = false;
+      disc_clamped_ = false;
+      disc_ejected_ = false;
+      if (loader_goal_ == LoaderGoal::GRAB ||
+          loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+        if (loader_goal_ == LoaderGoal::GRAB) {
+          printf("Told to GRAB, moving on\n");
+        } else {
+          printf("Told to SHOOT_AND_RESET, moving on\n");
+        }
+        loader_state_ = LoaderState::GRABBING;
+        loader_countdown_ = kGrabbingDelay;
+      } else {
+        break;
+      }
+    case LoaderState::GRABBING:
+      printf("Loader GRABBING %d\n", loader_countdown_);
+      // Closing the grabber.
+      loader_up_ = false;
+      disc_clamped_ = true;
+      disc_ejected_ = false;
+      if (loader_countdown_ > 0) {
+        --loader_countdown_;
+        break;
+      } else {
+        loader_state_ = LoaderState::GRABBED;
+      }
+    case LoaderState::GRABBED:
+      printf("Loader GRABBED\n");
+      // Grabber closed.
+      loader_up_ = false;
+      disc_clamped_ = true;
+      disc_ejected_ = false;
+      if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET) {
+        // TODO(aschuh): Only shoot if the shooter is up to speed.
+        // Seems like that would have us shooting a bit later than we could be,
+        // but it also probably spins back up real fast.
+        loader_state_ = LoaderState::LIFTING;
+        loader_countdown_ = kLiftingDelay;
+        printf("Told to SHOOT_AND_RESET, moving on\n");
+      } else if (loader_goal_ == LoaderGoal::READY) {
+        LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
+        printf("Can't go to ready when we have something grabbed.\n");
+        break;
+      } else {
+        break;
+      }
+    case LoaderState::LIFTING:
+      printf("Loader LIFTING %d\n", loader_countdown_);
+      // Lifting the disc.
+      loader_up_ = true;
+      disc_clamped_ = true;
+      disc_ejected_ = false;
+      if (loader_countdown_ > 0) {
+        --loader_countdown_;
+        break;
+      } else {
+        loader_state_ = LoaderState::LIFTED;
+      }
+    case LoaderState::LIFTED:
+      printf("Loader LIFTED\n");
+      // Disc lifted.  Time to eject it out.
+      loader_up_ = true;
+      disc_clamped_ = true;
+      disc_ejected_ = false;
+      loader_state_ = LoaderState::SHOOTING;
+      loader_countdown_ = kShootingDelay;
+    case LoaderState::SHOOTING:
+      printf("Loader SHOOTING %d\n", loader_countdown_);
+      // Ejecting the disc into the shooter.
+      loader_up_ = true;
+      disc_clamped_ = false;
+      disc_ejected_ = true;
+      if (loader_countdown_ > 0) {
+        --loader_countdown_;
+        break;
+      } else {
+        loader_state_ = LoaderState::SHOOT;
+      }
+    case LoaderState::SHOOT:
+      printf("Loader SHOOT\n");
+      // The disc has been shot.
+      loader_up_ = true;
+      disc_clamped_ = false;
+      disc_ejected_ = true;
+      loader_state_ = LoaderState::LOWERING;
+      loader_countdown_ = kLoweringDelay;
+    case LoaderState::LOWERING:
+      printf("Loader LOWERING %d\n", loader_countdown_);
+      // Lowering the loader back down.
+      loader_up_ = false;
+      disc_clamped_ = false;
+      disc_ejected_ = true;
+      if (loader_countdown_ > 0) {
+        --loader_countdown_;
+        break;
+      } else {
+        loader_state_ = LoaderState::LOWERED;
+      }
+    case LoaderState::LOWERED:
+      printf("Loader LOWERED\n");
+      // The indexer is lowered.
+      loader_up_ = false;
+      disc_clamped_ = false;
+      disc_ejected_ = false;
+      loader_state_ = LoaderState::READY;
+      // Once we have shot, we need to hang out in READY until otherwise
+      // notified.
+      loader_goal_ = LoaderGoal::READY;
       break;
   }
 
@@ -215,17 +470,20 @@
   wrist_loop_->Update(position != NULL, output == NULL);
 
   if (position) {
-    LOG(DEBUG, "pos=%f currently %f\n",
-        position->index_position, index_position);
+    LOG(DEBUG, "pos=%f\n", position->index_position);
     last_bottom_disc_detect_ = position->bottom_disc_detect;
   }
 
   status->hopper_disc_count = hopper_disc_count_;
   status->total_disc_count = total_disc_count_;
-
+  status->preloaded = (loader_state_ != LoaderState::READY);
 
   if (output) {
+    output->transfer_voltage = transfer_voltage;
     output->index_voltage = wrist_loop_->U(0, 0);
+    output->loader_up = loader_up_;
+    output->disc_clamped = disc_clamped_;
+    output->disc_ejected = disc_ejected_;
   }
 
   if (safe_to_change_state_) {