Wait for at least one index pulse while zeroing.
Change-Id: Ic5648d533bb9a9bab05c7683c7874b2c1de8fde4
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 51d1274..5f1d263 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -166,11 +166,48 @@
ZeroingEstimator estimator(
Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ MoveTo(&sim, &estimator, 3.1 * index_diff);
+
for (unsigned int i = 0; i < kSampleSize; i++) {
MoveTo(&sim, &estimator, 5.0 * index_diff);
}
+
ASSERT_NEAR(3.1 * index_diff, estimator.offset(), 0.001);
}
+TEST_F(ZeroingTest, WaitForIndexPulseAfterReset) {
+ double index_diff = 0.6;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.1 * index_diff, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+
+ // Make sure to fill up the averaging filter with samples.
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 3.1 * index_diff);
+ }
+
+ // Make sure we're not zeroed until we hit an index pulse.
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Trigger an index pulse; we should now be zeroed.
+ MoveTo(&sim, &estimator, 4.5 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+
+ // Reset the zeroing logic and supply a bunch of samples within the current
+ // index segment.
+ estimator.Reset();
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 4.2 * index_diff);
+ }
+
+ // Make sure we're not zeroed until we hit an index pulse.
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Trigger another index pulse; we should be zeroed again.
+ MoveTo(&sim, &estimator, 3.1 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+}
+
} // namespace zeroing
} // namespace frc971