commit 004ec817f863adceab1d79f199b5551ad38794be
author Brian Silverman <brians> Thu Sep 26 15:47:58 2013 -0700
committer Brian Silverman <brians> Thu Sep 26 15:47:58 2013 -0700
tree e44f01afd82e2935ecefcf698fc43cff17351db7
parent c540ab8350d03ae908534b2e79506036811332ba

added the bottom sensor too

frc971/control_loops/index/index.cc

diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 3932ab5..4ec77ba 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -109,7 +109,8 @@
 /*static*/ const int IndexMotor::kLiftingDelay = 2;
 /*static*/ const int IndexMotor::kLiftingTimeout = 65;
 /*static*/ const int IndexMotor::kShootingDelay = 10;
-/*static*/ const int IndexMotor::kLoweringDelay = 20;
+/*static*/ const int IndexMotor::kLoweringDelay = 4;
+/*static*/ const int IndexMotor::kLoweringTimeout = 120;
 
 // TODO(aschuh): Tune these.
 /*static*/ const double
@@ -900,6 +901,8 @@
       if (position->loader_top) {
         if (loader_countdown_ > 0) {
           --loader_countdown_;
+          loader_timeout_ = 0;
+          break;
         } else {
           loader_state_ = LoaderState::LIFTED;
         }
@@ -910,9 +913,10 @@
         if (loader_timeout_ > kLiftingTimeout) {
           LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
           loader_state_ = LoaderState::LIFTED;
+        } else {
+          break;
         }
       }
-      break;
     case LoaderState::LIFTED:
       LOG(DEBUG, "Loader LIFTED\n");
       // Disc lifted.  Time to eject it out.
@@ -941,6 +945,7 @@
       disc_ejected_ = true;
       loader_state_ = LoaderState::LOWERING;
       loader_countdown_ = kLoweringDelay;
+      loader_timeout_ = 0;
       --hopper_disc_count_;
       ++shot_disc_count_;
     case LoaderState::LOWERING:
@@ -949,11 +954,24 @@
       loader_up_ = false;
       disc_clamped_ = false;
       disc_ejected_ = true;
-      if (loader_countdown_ > 0) {
-        --loader_countdown_;
-        break;
+      if (position->loader_bottom) {
+        if (loader_countdown_ > 0) {
+          --loader_countdown_;
+          loader_timeout_ = 0;
+          break;
+        } else {
+          loader_state_ = LoaderState::LOWERED;
+        }
       } else {
-        loader_state_ = LoaderState::LOWERED;
+        // Restart the countdown if it bounces back up or something.
+        loader_countdown_ = kLoweringDelay;
+        ++loader_timeout_;
+        if (loader_timeout_ > kLoweringTimeout) {
+          LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
+          loader_state_ = LoaderState::LOWERED;
+        } else {
+          break;
+        }
       }
     case LoaderState::LOWERED:
       LOG(DEBUG, "Loader LOWERED\n");