Add a continuous absolute encoder zeroer
For swerve modules, we need to be able to zero a system where the
absolute encoder has a 1:1 ratio to the output and the output can spin
infinitely. Create an altered version of the AbsoluteEncoder zeroer to
do this.
Tests basically just steal from the absolute encoder tests, with an
extra test to check that it handles multiple revolutions correctly.
Change-Id: I0d2738499480f142bc5703de2c9cf6e345e3a896
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/zeroing/continuous_absolute_encoder_test.cc b/frc971/zeroing/continuous_absolute_encoder_test.cc
new file mode 100644
index 0000000..3869393
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder_test.cc
@@ -0,0 +1,198 @@
+#include "frc971/zeroing/continuous_absolute_encoder.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/wrap.h"
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::ContinuousAbsoluteEncoderZeroingConstants;
+
+class ContinuousAbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ ContinuousAbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ flatbuffers::FlatBufferBuilder fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+}
+
+// Makes sure that if the underlying mechanism moves by >1 revolution that the
+// continuous zeroing estimator handles it correctly.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ ContinuousEstimatorZeroesAcrossRevolution) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+
+ // Now, rotate by a full revolution, then stay still. We should stay zeroed.
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize; ++i) {
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+ }
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+}
+
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwards.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 10 * index_diff;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithNaN) {
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+ ASSERT_FALSE(estimator.error());
+
+ estimator.UpdateEstimate(*sensor_values);
+ ASSERT_TRUE(estimator.error());
+
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.Finish(estimator.GetEstimatorState(&fbb2));
+
+ const AbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteEncoderEstimatorState>(
+ fbb2.GetBufferPointer());
+
+ EXPECT_THAT(*state->errors(),
+ ::testing::ElementsAre(ZeroingError::LOST_ABSOLUTE_ENCODER));
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971