zeroing: Support first index pulse at non-zero positions.
Change-Id: I13cd11cfc46af27640e3fe7f63795475ae901f8a
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index d279318..9228331 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -11,6 +11,7 @@
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
index_pulse_count_after_reset_ = 0;
+ known_index_pos_ = constants.measured_index_position;
start_pos_samples_.reserve(max_sample_count_);
@@ -61,11 +62,13 @@
start_pos_ = start_average;
} else {
// We calculate an aproximation of the value of the last index position.
- double index_pos = start_average + info.latched_encoder;
+ // Also account for index pulses not lining up with integer multiples of
+ // the index_diff.
+ double index_pos = start_average + info.latched_encoder - known_index_pos_;
// We round index_pos to the closest valid value of the index.
double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
- // Now we reverse the first calculation to the accurate start position.
- start_pos_ = accurate_index_pos - info.latched_encoder;
+ // Now we reverse the first calculation to get the accurate start position.
+ start_pos_ = accurate_index_pos - info.latched_encoder + known_index_pos_;
// Now that we have an accurate starting position we can consider ourselves
// zeroed.
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 13be062..5566533 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -7,9 +7,6 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/constants.h"
-// TODO(pschrader): Support the ZeroingConstants::measured_index_position
-// parameter.
-//
// TODO(pschrader): Create an error API to flag faults/errors etc..
//
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
@@ -76,6 +73,9 @@
// The estimated starting position of the mechanism. We also call this the
// 'offset' in some contexts.
double start_pos_;
+ // The absolute position of any index pulse on the mechanism. This is used to
+ // account for the various ways the encoders get mounted into the robot.
+ double known_index_pos_;
// Flag for triggering logic that takes note of the current index pulse count
// after a reset. See `index_pulse_count_after_reset_'.
bool wait_for_index_pulse_;
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 5f1d263..6798123 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -209,5 +209,33 @@
ASSERT_TRUE(estimator.zeroed());
}
+TEST_F(ZeroingTest, TestNonZeroIndexPulseOffsets) {
+ const double index_diff = 0.9;
+ const double known_index_pos = 3.5 * index_diff;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.3 * index_diff, index_diff / 3.0, known_index_pos);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, known_index_pos});
+
+ // Make sure to fill up the averaging filter with samples.
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 3.3 * 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, 3.7 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
+ ASSERT_DOUBLE_EQ(3.7 * index_diff, estimator.position());
+
+ // Trigger one more index pulse and check the offset.
+ MoveTo(&sim, &estimator, 4.7 * index_diff);
+ ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
+ ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.position());
+}
+
} // namespace zeroing
} // namespace frc971