Added Zeroing::offset_ready() helper and TODO.
Change-Id: I1bd1d0e667bb9081896f6f48030b8fcfb95cdc79
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index e54446a..5184ad4 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -84,8 +84,8 @@
// If there are no index pulses to use or we don't have enough samples yet to
// have a well-filtered starting position then we use the filtered value as
// our best guess.
- if (!zeroed_ && (info.index_pulses == last_used_index_pulse_count_ ||
- offset_ratio_ready() < 1.0)) {
+ if (!zeroed_ &&
+ (info.index_pulses == last_used_index_pulse_count_ || !offset_ready())) {
start_pos_ = start_average;
} else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses) {
// Note the accurate start position and the current index pulse count so
@@ -93,6 +93,10 @@
// resilient to corrupted intermediate data.
start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
last_used_index_pulse_count_ = info.index_pulses;
+
+ // TODO(austin): Reject encoder positions which have x% error rather than
+ // rounding to the closest index pulse.
+
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = start_pos_;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 4176f48..433745a 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -63,6 +63,11 @@
return start_pos_samples_.size() / static_cast<double>(max_sample_count_);
}
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const {
+ return start_pos_samples_.size() == max_sample_count_;
+ }
+
private:
// This function calculates the start position given the internal state and
// the provided `latched_encoder' value.
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index b4c9813..1134259 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -158,6 +158,13 @@
MoveTo(&sim, &estimator, 3.5 * index_diff);
}
ASSERT_NEAR(0.5, estimator.offset_ratio_ready(), 0.001);
+ ASSERT_FALSE(estimator.offset_ready());
+
+ for (unsigned int i = 0; i < kSampleSize / 2; i++) {
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ }
+ ASSERT_NEAR(1.0, estimator.offset_ratio_ready(), 0.001);
+ ASSERT_TRUE(estimator.offset_ready());
}
TEST_F(ZeroingTest, TestOffset) {