Detect slipping on the PulseIndexZeroingEstimator
This only works after zeroing, not during, but it's a great start.
Change-Id: Ib3c9797a456b4181b82b47263020ba2120faf3d5
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index a4ba3d7..819fad0 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -400,6 +400,7 @@
constants.index_difference = 10.0;
constants.measured_index_position = 20.0;
constants.known_index_pulse = 1;
+ constants.allowable_encoder_error = 0.01;
PositionSensorSimulator sim(constants.index_difference);
@@ -447,6 +448,57 @@
estimator.GetEstimatorState().position);
}
+// Tests that we can detect when an index pulse occurs where we didn't expect
+// it to for the PulseIndexZeroingEstimator.
+TEST_F(ZeroingTest, TestRelativeEncoderSlipping) {
+ EncoderPlusIndexZeroingConstants constants;
+ constants.index_pulse_count = 3;
+ constants.index_difference = 10.0;
+ constants.measured_index_position = 20.0;
+ constants.known_index_pulse = 1;
+ constants.allowable_encoder_error = 0.05;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos =
+ constants.measured_index_position + 0.5 * constants.index_difference;
+
+ for (double direction : {1.0, -1.0}) {
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position);
+
+ PulseIndexZeroingEstimator estimator(constants);
+
+ // Zero the estimator.
+ MoveTo(&sim, &estimator, start_pos - 1 * constants.index_difference);
+ MoveTo(
+ &sim, &estimator,
+ start_pos - constants.index_pulse_count * constants.index_difference);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+
+ // We have a 5% allowable error so we slip a little bit each time and make
+ // sure that the index pulses are still accepted.
+ for (double error = 0.00;
+ ::std::abs(error) < constants.allowable_encoder_error;
+ error += 0.01 * direction) {
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position +
+ error * constants.index_difference);
+ MoveTo(&sim, &estimator, start_pos - constants.index_difference);
+ EXPECT_FALSE(estimator.error());
+ }
+
+ // As soon as we hit cross the error margin, we should trigger an error.
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position +
+ constants.allowable_encoder_error * 1.1 *
+ constants.index_difference * direction);
+ MoveTo(&sim, &estimator, start_pos - constants.index_difference);
+ ASSERT_TRUE(estimator.error());
+ }
+}
+
// Tests that an error is detected when the starting position changes too much.
TEST_F(ZeroingTest, TestHallEffectZeroing) {
constants::HallEffectZeroingConstants constants;