commit c5ef1bbed074c81ced44cfcad32d6e630f58158f
author Austin Schuh <austin.linux@gmail.com> Sun Mar 10 00:42:05 2013 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 10 00:42:05 2013 -0800
tree dcedf392134e03d401a1f3ee547e67a713ddc626
parent cc297022ec2813489e14b4f3d3df6e512945d75d

Indexer didn't deal with cRIO starting up with a nonzero encoder position.

frc971/control_loops/index/index.cc

diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index b5ff7f8..14c9800 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -287,6 +287,7 @@
     // so we don't always spin the indexer to the 0 position before starting.
     if (no_prior_position_) {
       wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
+      wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
       no_prior_position_ = false;
       last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
       last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
@@ -321,6 +322,7 @@
            frisbee != frisbees_.end(); ++frisbee) {
         frisbee->OffsetDisc(disc_offset);
       }
+      wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
     }
     missing_position_count_ = 0;
   } else {

frc971/control_loops/index/index_lib_test.cc

diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index c5ae309..ab3597c 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -180,6 +180,7 @@
     }
 
     if (shrunk_time) {
+      EXPECT_LT(0, transfer_roller_velocity);
       position_ = IndexMotor::kIndexStartPosition;
     } else {
       position_ += disc_dx;
@@ -239,7 +240,11 @@
     }
 
     if (shrunk_time) {
-      position_ = IndexMotor::kGrabberStartPosition;
+      if (index_roller_velocity > 0) {
+        position_ = IndexMotor::kGrabberStartPosition;
+      } else {
+        position_ = IndexMotor::kIndexStartPosition;
+      }
     } else {
       position_ += index_dx;
     }
@@ -699,7 +704,6 @@
     // Spin it up.
     SimulateNCycles(100);
 
-    EXPECT_EQ(0, index_motor_plant_.index_roller_position());
     my_index_loop_.status.FetchLatest();
     EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
 
@@ -1255,6 +1259,22 @@
   EXPECT_EQ(0u, index_motor_.frisbees_.size());
 }
 
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CRIOReboot) {
+  index_motor_plant_.index_plant_->Y(0, 0) = 5000.0;
+  index_motor_plant_.index_plant_->X(0, 0) = 5000.0;
+  LoadNDiscs(1);
+  my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+  SimulateNCycles(200);
+  my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+  SimulateNCycles(10);
+  my_index_loop_.output.FetchLatest();
+  EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+  my_index_loop_.status.FetchLatest();
+  EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+  EXPECT_EQ(1, my_index_loop_.status->hopper_disc_count);
+}
+
 }  // namespace testing
 }  // namespace control_loops
 }  // namespace frc971