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/constants.h b/frc971/constants.h
index 7c2121d..7f1f1d5 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -89,6 +89,33 @@
double allowable_encoder_error;
};
+struct AbsoluteAndAbsoluteEncoderZeroingConstants {
+ // The number of samples in the moving average filter.
+ size_t average_filter_size;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ double one_revolution_distance;
+ // Measured absolute position of the encoder when at zero.
+ double measured_absolute_position;
+
+ // The distance that the single turn absolute encoder needs to complete a full
+ // rotation.
+ double single_turn_one_revolution_distance;
+ // Measured absolute position of the single turn encoder when at zero.
+ double single_turn_measured_absolute_position;
+ // Position of the middle of the range of motion in output coordinates.
+ double single_turn_middle_position;
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ double zeroing_threshold;
+ // Buffer size for deciding if we are moving.
+ size_t moving_buffer_size;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
+};
+
// Defines a range of motion for a subsystem.
// These are all absolute positions in scaled units.
struct Range {
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index 4377541..f510085 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -130,6 +130,23 @@
absolute_position:double;
}
+// The internal state of a zeroing estimator.
+table AbsoluteAndAbsoluteEncoderEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ error:bool (id: 0);
+ // If the joint has seen an index pulse and is zeroed.
+ zeroed:bool (id: 1);
+ // The estimated position of the joint.
+ position:double (id: 2);
+
+ // The estimated absolute position of the encoder. This is filtered, so it
+ // can be easily used when zeroing.
+ absolute_position:double (id: 3);
+
+ // Estimated absolute position of the single turn absolute encoder.
+ single_turn_absolute_position:double (id: 4);
+}
+
table RelativeEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
error:bool;
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