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/constants.h b/frc971/constants.h
index 4f7cb36..cf53287 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -70,6 +70,28 @@
struct RelativeEncoderZeroingConstants {};
+struct ContinuousAbsoluteEncoderZeroingConstants {
+ // 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.
+ // It is presumed that this will always be 2 * pi for any subsystem using this
+ // class, unless you have a continuous system that for some reason doesn't
+ // have a logical period of 1 revolution in radians.
+ double one_revolution_distance;
+ // Measured absolute position of the encoder when at zero.
+ double measured_absolute_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 a revolution
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
+};
+
struct AbsoluteEncoderZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index d50f181..07e9852 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -63,6 +63,7 @@
srcs = [
"absolute_and_absolute_encoder.cc",
"absolute_encoder.cc",
+ "continuous_absolute_encoder.cc",
"hall_effect_and_position.cc",
"pot_and_absolute_encoder.cc",
"pot_and_index.cc",
@@ -71,6 +72,7 @@
hdrs = [
"absolute_and_absolute_encoder.h",
"absolute_encoder.h",
+ "continuous_absolute_encoder.h",
"hall_effect_and_position.h",
"pot_and_absolute_encoder.h",
"pot_and_index.h",
@@ -93,6 +95,7 @@
srcs = [
"absolute_and_absolute_encoder_test.cc",
"absolute_encoder_test.cc",
+ "continuous_absolute_encoder_test.cc",
"hall_effect_and_position_test.cc",
"pot_and_absolute_encoder_test.cc",
"pot_and_index_test.cc",
diff --git a/frc971/zeroing/continuous_absolute_encoder.cc b/frc971/zeroing/continuous_absolute_encoder.cc
new file mode 100644
index 0000000..a47a491
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder.cc
@@ -0,0 +1,169 @@
+#include "frc971/zeroing/continuous_absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "aos/containers/error_list.h"
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+ContinuousAbsoluteEncoderZeroingEstimator::
+ ContinuousAbsoluteEncoderZeroingEstimator(
+ const constants::ContinuousAbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void ContinuousAbsoluteEncoderZeroingEstimator::Reset() {
+ zeroed_ = false;
+ error_ = false;
+ first_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ position_ = 0.0;
+ nan_samples_ = 0;
+ relative_to_absolute_offset_samples_.clear();
+ move_detector_.Reset();
+}
+
+// The math here is a bit backwards, but I think it'll be less error prone that
+// way and more similar to the version with a pot as well.
+//
+// We start by unwrapping the absolute encoder using the relative encoder. This
+// puts us in a non-wrapping space and lets us average a bit easier. From
+// there, we can compute an offset and wrap ourselves back such that we stay
+// close to the middle value.
+//
+// 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 ContinuousAbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const AbsolutePosition &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.";
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
+ error_ = true;
+ } else {
+ ++nan_samples_;
+ VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
+ if (nan_samples_ >= constants_.average_filter_size) {
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
+ error_ = true;
+ zeroed_ = true;
+ }
+ }
+ // Throw some dummy values in for now.
+ filtered_absolute_encoder_ = info.absolute_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();
+
+ // adjusted_* numbers are nominally in the desired output frame.
+ const double adjusted_absolute_encoder =
+ sample.absolute_encoder - constants_.measured_absolute_position;
+
+ // Note: If are are near the breakpoint of the absolute encoder, this number
+ // will be jitter between numbers that are ~one_revolution_distance apart.
+ // For that reason, we rewrap it so that we are not near that boundary.
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - sample.encoder;
+
+ // To avoid the aforementioned jitter, choose a base value to use for
+ // wrapping. When we have no prior samples, just use the current offset.
+ // Otherwise, we use an arbitrary prior offset (the stored offsets will all
+ // already be wrapped).
+ const double relative_to_absolute_offset_wrap_base =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? relative_to_absolute_offset
+ : relative_to_absolute_offset_samples_[0];
+
+ const double relative_to_absolute_offset_wrapped =
+ UnWrap(relative_to_absolute_offset_wrap_base,
+ relative_to_absolute_offset, constants_.one_revolution_distance);
+
+ 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) {
+ relative_to_absolute_offset_samples_.push_back(
+ relative_to_absolute_offset_wrapped);
+ } else {
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset_wrapped;
+ }
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ // Compute the average offset between the absolute encoder and relative
+ // encoder. Because we just pushed a value, the size() will never be zero.
+ offset_ =
+ ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(), 0.0) /
+ relative_to_absolute_offset_samples_.size();
+
+ // To provide a value that can be used to estimate the
+ // measured_absolute_position when zeroing, we just need to output the
+ // current absolute encoder value. We could make use of the averaging
+ // implicit in offset_ to reduce the noise on this slightly.
+ filtered_absolute_encoder_ = sample.absolute_encoder;
+
+ 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;
+ errors_.Set(ZeroingError::OFFSET_MOVED_TOO_FAR);
+ error_ = true;
+ }
+
+ zeroed_ = true;
+ }
+ }
+
+ // Update the position. Wrap it to reflect the fact that we do not have
+ // sufficient information to disambiguate which revolution we are on (also,
+ // since this value is primarily meant for debugging, this makes it easier to
+ // see that the device is actually at zero without having to divide by 2 *
+ // pi).
+ position_ =
+ Wrap(0.0, offset_ + info.encoder(), constants_.one_revolution_distance);
+}
+
+flatbuffers::Offset<ContinuousAbsoluteEncoderZeroingEstimator::State>
+ContinuousAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<flatbuffers::Vector<ZeroingError>> errors_offset =
+ errors_.ToFlatbuffer(fbb);
+
+ 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_errors(errors_offset);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/continuous_absolute_encoder.h b/frc971/zeroing/continuous_absolute_encoder.h
new file mode 100644
index 0000000..b912e84
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder.h
@@ -0,0 +1,98 @@
+#ifndef FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
+#define FRC971_ZEROING_CONTINUOUS_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 spins continuously. The
+// absolute encoder must have a 1:1 ratio to the output.
+// The provided offset(), when added to incremental encoder, may return a value
+// outside of +/- pi. The user is responsible for handling wrapping.
+class ContinuousAbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<
+ AbsolutePosition,
+ constants::ContinuousAbsoluteEncoderZeroingConstants,
+ AbsoluteEncoderEstimatorState> {
+ public:
+ explicit ContinuousAbsoluteEncoderZeroingEstimator(
+ const constants::ContinuousAbsoluteEncoderZeroingConstants &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 AbsolutePosition &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;
+ }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ struct PositionStruct {
+ PositionStruct(const AbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double absolute_encoder;
+ double encoder;
+ };
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::ContinuousAbsoluteEncoderZeroingConstants 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;
+
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+
+ MoveDetector<PositionStruct, AbsolutePosition> move_detector_;
+
+ // 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;
+
+ // Number of NANs we've seen in a row.
+ size_t nan_samples_ = 0;
+
+ // The filtered position.
+ double position_ = 0.0;
+
+ // Marker to track what kind of error has occured.
+ aos::ErrorList<ZeroingError> errors_;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
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
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index f7b52a6..b0e6825 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -176,6 +176,7 @@
// reverse dependencies to #include what they actually need...
#include "frc971/zeroing/absolute_and_absolute_encoder.h"
#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/continuous_absolute_encoder.h"
#include "frc971/zeroing/hall_effect_and_position.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/pot_and_index.h"