Create zeroing estimator for absolute and absolute encoder
The same as pot and absolute encoder, but with an absolute encoder that
doesn't do index pulses, but can only go through one rotation.
Change-Id: I4b41df7e93ac4454c9aec0fee3c072b4bc9a57c1
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 5955f8b..df85290 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -64,6 +64,7 @@
"pot_and_absolute_encoder.cc",
"pot_and_index.cc",
"pulse_index.cc",
+ "absolute_and_absolute_encoder.cc",
],
hdrs = [
"absolute_encoder.h",
@@ -72,6 +73,7 @@
"pot_and_index.h",
"pulse_index.h",
"zeroing.h",
+ "absolute_and_absolute_encoder.h",
],
deps = [
":wrap",
@@ -91,6 +93,7 @@
"pot_and_index_test.cc",
"pulse_index_test.cc",
"relative_encoder_test.cc",
+ "absolute_and_absolute_encoder_test.cc",
"zeroing_test.h",
],
deps = [
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.cc b/frc971/zeroing/absolute_and_absolute_encoder.cc
new file mode 100644
index 0000000..54a5e78
--- /dev/null
+++ b/frc971/zeroing/absolute_and_absolute_encoder.cc
@@ -0,0 +1,216 @@
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+AbsoluteAndAbsoluteEncoderZeroingEstimator::
+ AbsoluteAndAbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteAndAbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void AbsoluteAndAbsoluteEncoderZeroingEstimator::Reset() {
+ first_offset_ = 0.0;
+ single_turn_to_relative_encoder_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ filtered_position_ = 0.0;
+ position_ = 0.0;
+ zeroed_ = false;
+ nan_samples_ = 0;
+ relative_to_absolute_offset_samples_.clear();
+ offset_samples_.clear();
+ move_detector_.Reset();
+ error_ = false;
+}
+
+// So, this needs to be a multistep process. We need to first estimate the
+// offset between the absolute encoder and the relative encoder. That process
+// should get us an absolute number which is off by integer multiples of the
+// distance/rev. In parallel, we can estimate the offset between the single
+// turn encoder and encoder. When both estimates have converged, we can then
+// compute the offset in a cycle, and which cycle, which gives us the accurate
+// global offset.
+//
+// It's tricky to compute the offset between the absolute and relative encoder.
+// We need to compute this inside 1 revolution. The easiest way to do this
+// would be to wrap the encoder, subtract the two of them, and then average the
+// result. That will struggle when they are off by PI. Instead, we need to
+// wrap the number to +- PI from the current averaged offset.
+//
+// To guard against the robot moving while updating estimates, buffer a number
+// of samples and check that the buffered samples are not different than the
+// zeroing threshold. At any point that the samples differ too much, do not
+// update estimates based on those samples.
+void AbsoluteAndAbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const AbsoluteAndAbsolutePosition &info) {
+ // Check for Abs Encoder NaN value that would mess up the rest of the zeroing
+ // code below. NaN values are given when the Absolute Encoder is disconnected.
+ if (::std::isnan(info.absolute_encoder())) {
+ if (zeroed_) {
+ VLOG(1) << "NAN on absolute encoder.";
+ error_ = true;
+ } else {
+ ++nan_samples_;
+ VLOG(1) << "NAN on absolute encoder while zeroing" << nan_samples_;
+ if (nan_samples_ >= constants_.average_filter_size) {
+ error_ = true;
+ zeroed_ = true;
+ }
+ }
+ // Throw some dummy values in for now.
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ filtered_position_ =
+ single_turn_to_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
+ return;
+ }
+
+ const bool moving = move_detector_.Update(info, constants_.moving_buffer_size,
+ constants_.zeroing_threshold);
+
+ if (!moving) {
+ const PositionStruct &sample = move_detector_.GetSample();
+
+ // Compute the average offset between the absolute encoder and relative
+ // encoder. If we have 0 samples, assume it is 0.
+ double average_relative_to_absolute_offset =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? 0.0
+ : ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(),
+ 0.0) /
+ relative_to_absolute_offset_samples_.size();
+
+ const double adjusted_incremental_encoder =
+ sample.encoder + average_relative_to_absolute_offset;
+
+ // Now, compute the nearest absolute encoder value to the offset relative
+ // encoder position.
+ const double adjusted_absolute_encoder =
+ UnWrap(adjusted_incremental_encoder,
+ sample.absolute_encoder - constants_.measured_absolute_position,
+ constants_.one_revolution_distance);
+
+ // We can now compute the offset now that we have unwrapped the absolute
+ // encoder.
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - sample.encoder;
+
+ // Add the sample and update the average with the new reading.
+ const size_t relative_to_absolute_offset_samples_size =
+ relative_to_absolute_offset_samples_.size();
+ if (relative_to_absolute_offset_samples_size <
+ constants_.average_filter_size) {
+ average_relative_to_absolute_offset =
+ (average_relative_to_absolute_offset *
+ relative_to_absolute_offset_samples_size +
+ relative_to_absolute_offset) /
+ (relative_to_absolute_offset_samples_size + 1);
+
+ relative_to_absolute_offset_samples_.push_back(
+ relative_to_absolute_offset);
+ } else {
+ average_relative_to_absolute_offset -=
+ relative_to_absolute_offset_samples_[samples_idx_] /
+ relative_to_absolute_offset_samples_size;
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset;
+ average_relative_to_absolute_offset +=
+ relative_to_absolute_offset /
+ relative_to_absolute_offset_samples_size;
+ }
+
+ const double adjusted_single_turn_absolute_encoder =
+ UnWrap(constants_.single_turn_middle_position,
+ sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position,
+ constants_.single_turn_one_revolution_distance);
+
+ // Now compute the offset between the pot and relative encoder.
+ if (offset_samples_.size() < constants_.average_filter_size) {
+ offset_samples_.push_back(adjusted_single_turn_absolute_encoder -
+ sample.encoder);
+ } else {
+ offset_samples_[samples_idx_] =
+ adjusted_single_turn_absolute_encoder - sample.encoder;
+ }
+
+ // Drop the oldest sample when we run this function the next time around.
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ single_turn_to_relative_encoder_offset_ =
+ ::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
+ offset_samples_.size();
+
+ offset_ = UnWrap(sample.encoder + single_turn_to_relative_encoder_offset_,
+ average_relative_to_absolute_offset + sample.encoder,
+ constants_.one_revolution_distance) -
+ sample.encoder;
+
+ // Reverse the math for adjusted_absolute_encoder to compute the absolute
+ // encoder. Do this by taking the adjusted encoder, and then subtracting off
+ // the second argument above, and the value that was added by Wrap.
+ filtered_absolute_encoder_ =
+ ((sample.encoder + average_relative_to_absolute_offset) -
+ (-constants_.measured_absolute_position +
+ (adjusted_absolute_encoder -
+ (sample.absolute_encoder - constants_.measured_absolute_position))));
+
+ // TODO(Ravago): this is impossible to read.
+ filtered_single_turn_absolute_encoder_ =
+ ((sample.encoder + single_turn_to_relative_encoder_offset_) -
+ (-constants_.single_turn_measured_absolute_position +
+ (adjusted_single_turn_absolute_encoder -
+ (sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position))));
+
+ if (offset_ready()) {
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
+
+ if (::std::abs(first_offset_ - offset_) >
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance) {
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
+ error_ = true;
+ }
+
+ zeroed_ = true;
+ }
+ }
+
+ // Update the position.
+ filtered_position_ = single_turn_to_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
+}
+
+flatbuffers::Offset<AbsoluteAndAbsoluteEncoderZeroingEstimator::State>
+AbsoluteAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ builder.add_single_turn_absolute_position(
+ filtered_single_turn_absolute_encoder_);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.h b/frc971/zeroing/absolute_and_absolute_encoder.h
new file mode 100644
index 0000000..7d56f2b
--- /dev/null
+++ b/frc971/zeroing/absolute_and_absolute_encoder.h
@@ -0,0 +1,110 @@
+#ifndef frc971_zeroing_absolute_and_absolute_encoder_h_
+#define frc971_zeroing_absolute_and_absolute_encoder_h_
+
+#include <vector>
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Estimates the position with an absolute encoder which also reports
+// incremental counts and an absolute encoder that's not allowed to turn more
+// than once. This is like the PotAndAbsoluteEncoderZeroingEstimator, but it
+// uses the single turn absolute encoder instead of the potentiometer.
+class AbsoluteAndAbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<
+ AbsoluteAndAbsolutePosition,
+ constants::AbsoluteAndAbsoluteEncoderZeroingConstants,
+ AbsoluteAndAbsoluteEncoderEstimatorState> {
+ public:
+ explicit AbsoluteAndAbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteAndAbsoluteEncoderZeroingConstants &constants);
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Updates the sensor values for the zeroing logic.
+ void UpdateEstimate(const AbsoluteAndAbsolutePosition &info) override;
+
+ void TriggerError() override { error_ = true; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const override {
+ return relative_to_absolute_offset_samples_.size() ==
+ constants_.average_filter_size &&
+ offset_samples_.size() == constants_.average_filter_size;
+ }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ struct PositionStruct {
+ PositionStruct(const AbsoluteAndAbsolutePosition &position_buffer)
+ : single_turn_absolute_encoder(
+ position_buffer.single_turn_absolute_encoder()),
+ absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double single_turn_absolute_encoder;
+ double absolute_encoder;
+ double encoder;
+ };
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::AbsoluteAndAbsoluteEncoderZeroingConstants constants_;
+
+ // True if the mechanism is zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+ // The first valid offset we recorded. This is only set after zeroed_ first
+ // changes to true.
+ double first_offset_;
+
+ // The filtered absolute encoder. This is used in the status for calibration.
+ double filtered_absolute_encoder_ = 0.0;
+
+ // The filtered single turn absolute encoder. This is used in the status for
+ // calibration.
+ double filtered_single_turn_absolute_encoder_ = 0.0;
+
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+
+ // Offset between the single turn absolute encoder and relative encoder
+ // position.
+ ::std::vector<double> offset_samples_;
+
+ MoveDetector<PositionStruct, AbsoluteAndAbsolutePosition> move_detector_;
+
+ // Estimated offset between the single turn encoder and relative encoder.
+ double single_turn_to_relative_encoder_offset_ = 0;
+ // Estimated start position of the mechanism
+ double offset_ = 0;
+ // The next position in 'relative_to_absolute_offset_samples_' and
+ // 'encoder_samples_' to be used to store the next sample.
+ int samples_idx_ = 0;
+
+ size_t nan_samples_ = 0;
+
+ // The unzeroed filtered position.
+ double filtered_position_ = 0.0;
+ // The filtered position.
+ double position_ = 0.0;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_ABSOLUTE_AND_ABSOLUTE_ENCODER_H_
diff --git a/frc971/zeroing/absolute_and_absolute_encoder_test.cc b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
new file mode 100644
index 0000000..cc1872d
--- /dev/null
+++ b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
@@ -0,0 +1,258 @@
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::AbsoluteAndAbsoluteEncoderZeroingConstants;
+using EstimatorState = AbsoluteAndAbsoluteEncoderZeroingEstimator::State;
+
+class AbsoluteAndAbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ AbsoluteAndAbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsoluteAndAbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingWithoutMovement) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ // Middle position for the single turn absolute encoder.
+ const double single_turn_middle_position = full_range * 0.5;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator 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());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwards.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingIgnoresNAN) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ // Middle position for the single turn absolute encoder.
+ const double single_turn_middle_position = full_range * 0.5;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreateAbsoluteAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*flatbuffers::GetRoot<AbsoluteAndAbsolutePosition>(
+ fbb.GetBufferPointer()));
+ }
+
+ 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());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingWithMovement) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ // Middle position for the single turn absolute encoder.
+ const double single_turn_middle_position = full_range * 0.5;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * distance_per_revolution);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * distance_per_revolution);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingWithNaN) {
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 1, 0.3, 2.5, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ FBB fbb;
+ fbb.Finish(CreateAbsoluteAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsoluteAndAbsolutePosition>(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());
+}
+
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingState) {
+ const double full_range = 4.0;
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+ const double start_pos = 2.1;
+ const double single_turn_middle_position = full_range * 0.5;
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ const double position = 2.7;
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, position);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, position);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+
+ FBB fbb;
+
+ fbb.Finish(estimator.GetEstimatorState(&fbb));
+
+ const EstimatorState *state =
+ flatbuffers::GetRoot<EstimatorState>(fbb.GetBufferPointer());
+
+ EXPECT_NEAR(state->position(), position, 1e-10);
+
+ // (position + measured_absolute_position) % distance_per_revolution
+ // (2.7 + 0.3) % 1
+ EXPECT_NEAR(state->absolute_position(), 0.0, 1e-10);
+
+ // (position + single_turn_measured_absolute_position) %
+ // single_turn_distance_per_revolution
+ // (2.7 + 1.6) % 4
+ EXPECT_NEAR(state->single_turn_absolute_position(), 0.3, 1e-10);
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971