commit 3cd11c5123056db87b2c9a98672a4fb9cd1baf4c
author Adam Snaider <adsnaider@gmail.com> Mon Feb 16 02:16:09 2015 +0000
committer Adam Snaider <adsnaider@gmail.com> Sun Mar 01 01:26:29 2015 +0000
tree 96356c01bd0401274a5593b11f4653545c428c97
parent 0a02d7d5c7148d4cc5480ee72225dca7c325a04d

Added detection of bad index zeroing pulse.

Change-Id: I1607ce0d37b336136a4a278afc8a0f2897d1c597

frc971/zeroing/zeroing.cc

diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 9b2347d..d8c77e8 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -6,8 +6,8 @@
 namespace frc971 {
 namespace zeroing {
 
-void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
-                            EstimatorState *state) {
+void PopulateEstimatorState(const zeroing::ZeroingEstimator& estimator,
+                            EstimatorState* state) {
   state->error = estimator.error();
   state->zeroed = estimator.zeroed();
   state->position = estimator.position();
@@ -18,9 +18,8 @@
   index_diff_ = constants.index_difference;
   max_sample_count_ = constants.average_filter_size;
   known_index_pos_ = constants.measured_index_position;
-
+  allowable_encoder_error_ = constants.allowable_encoder_error;
   start_pos_samples_.reserve(max_sample_count_);
-
   Reset();
 }
 
@@ -31,6 +30,7 @@
   zeroed_ = false;
   wait_for_index_pulse_ = true;
   last_used_index_pulse_count_ = 0;
+  first_start_pos_ = 0.0;
   error_ = false;
 }
 
@@ -93,10 +93,28 @@
     // resilient to corrupted intermediate data.
     start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
     last_used_index_pulse_count_ = info.index_pulses;
+    // Save the first starting position.
+    if (!zeroed_) {
+      first_start_pos_ = start_pos_;
+      LOG(INFO, "latching start position %f\n", first_start_pos_);
+    }
 
     // Now that we have an accurate starting position we can consider ourselves
     // zeroed.
     zeroed_ = true;
+    // Throw an error if first_start_pos is bigger/smaller than
+    // allowable_encoder_error_ * index_diff +
+    // start_pos.
+    if (::std::abs(first_start_pos_ - start_pos_) >
+        allowable_encoder_error_ * index_diff_) {
+      if (!error_) {
+        LOG(ERROR,
+            "Encoder ticks out of range since last index pulse. first start "
+            "position: %f recent starting position: %f\n",
+            first_start_pos_, start_pos_);
+        error_ = true;
+      }
+    }
   }
 
   pos_ = start_pos_ + info.encoder;