Split //frc971/zeroing into multiple files
It's still a single target, and one massive header file. We should
actually split that up in a future refactoring. However, this does make
it easier to find the relevant parts within each file.
Change-Id: I7abc26f2e3d88da4558f54d56e6db4233cc4c30f
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 75d610a..5955f8b 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -59,9 +59,18 @@
cc_library(
name = "zeroing",
srcs = [
- "zeroing.cc",
+ "absolute_encoder.cc",
+ "hall_effect_and_position.cc",
+ "pot_and_absolute_encoder.cc",
+ "pot_and_index.cc",
+ "pulse_index.cc",
],
hdrs = [
+ "absolute_encoder.h",
+ "hall_effect_and_position.h",
+ "pot_and_absolute_encoder.h",
+ "pot_and_index.h",
+ "pulse_index.h",
"zeroing.h",
],
deps = [
@@ -76,11 +85,16 @@
cc_test(
name = "zeroing_test",
srcs = [
- "zeroing_test.cc",
+ "absolute_encoder_test.cc",
+ "hall_effect_and_position_test.cc",
+ "pot_and_absolute_encoder_test.cc",
+ "pot_and_index_test.cc",
+ "pulse_index_test.cc",
+ "relative_encoder_test.cc",
+ "zeroing_test.h",
],
deps = [
":zeroing",
- "//aos:die",
"//aos/testing:googletest",
"//aos/testing:test_shm",
"//frc971/control_loops:control_loops_fbs",
diff --git a/frc971/zeroing/absolute_encoder.cc b/frc971/zeroing/absolute_encoder.cc
new file mode 100644
index 0000000..ffdf9da
--- /dev/null
+++ b/frc971/zeroing/absolute_encoder.cc
@@ -0,0 +1,178 @@
+#include "frc971/zeroing/absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+AbsoluteEncoderZeroingEstimator::AbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void AbsoluteEncoderZeroingEstimator::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 AbsoluteEncoderZeroingEstimator::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.";
+ 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();
+ 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();
+
+ // Now, compute the estimated absolute position using the previously
+ // estimated offset and the incremental encoder.
+ const double adjusted_incremental_encoder =
+ sample.encoder + average_relative_to_absolute_offset;
+
+ // Now, compute the absolute encoder value nearest 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;
+ }
+
+ // Drop the oldest sample when we run this function the next time around.
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ // And our offset is the offset that gives us the position within +- ord/2
+ // of the middle position.
+ offset_ = Wrap(constants_.middle_position,
+ 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))));
+
+ 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.
+ position_ = offset_ + info.encoder();
+}
+
+flatbuffers::Offset<AbsoluteEncoderZeroingEstimator::State>
+AbsoluteEncoderZeroingEstimator::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_);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/absolute_encoder.h b/frc971/zeroing/absolute_encoder.h
new file mode 100644
index 0000000..0021e13
--- /dev/null
+++ b/frc971/zeroing/absolute_encoder.h
@@ -0,0 +1,93 @@
+#ifndef FRC971_ZEROING_ABSOLUTE_ENCODER_H_
+#define FRC971_ZEROING_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. The absolute encoder can't spin more than one
+// revolution.
+class AbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<AbsolutePosition,
+ constants::AbsoluteEncoderZeroingConstants,
+ AbsoluteEncoderEstimatorState> {
+ public:
+ explicit AbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteEncoderZeroingConstants &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::AbsoluteEncoderZeroingConstants 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;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_ABSOLUTE_ENCODER_H_
diff --git a/frc971/zeroing/absolute_encoder_test.cc b/frc971/zeroing/absolute_encoder_test.cc
new file mode 100644
index 0000000..38ce069
--- /dev/null
+++ b/frc971/zeroing/absolute_encoder_test.cc
@@ -0,0 +1,145 @@
+#include "frc971/zeroing/absolute_encoder.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::AbsoluteEncoderZeroingConstants;
+
+class AbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ AbsoluteEncoderZeroingEstimator *estimator, double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(AbsoluteEncoderZeroingTest, TestAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double kMiddlePosition = 2.5;
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ kMiddlePosition, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ AbsoluteEncoderZeroingEstimator 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(AbsoluteEncoderZeroingTest, TestAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+ const double kMiddlePosition = 2.5;
+
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ kMiddlePosition, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ AbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ FBB 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());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(AbsoluteEncoderZeroingTest, TestAbsoluteEncoderZeroingWithMovement) {
+ 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;
+ const double kMiddlePosition = 2.5;
+
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ kMiddlePosition, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ AbsoluteEncoderZeroingEstimator 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(AbsoluteEncoderZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
+ AbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 1.0, 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ AbsoluteEncoderZeroingEstimator estimator(constants);
+
+ FBB 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());
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/hall_effect_and_position.cc b/frc971/zeroing/hall_effect_and_position.cc
new file mode 100644
index 0000000..e074668
--- /dev/null
+++ b/frc971/zeroing/hall_effect_and_position.cc
@@ -0,0 +1,127 @@
+#include "frc971/zeroing/hall_effect_and_position.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace zeroing {
+
+HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants)
+ : constants_(constants) {
+ Reset();
+}
+
+void HallEffectAndPositionZeroingEstimator::Reset() {
+ offset_ = 0.0;
+ min_low_position_ = ::std::numeric_limits<double>::max();
+ max_low_position_ = ::std::numeric_limits<double>::lowest();
+ zeroed_ = false;
+ initialized_ = false;
+ last_used_posedge_count_ = 0;
+ cycles_high_ = 0;
+ high_long_enough_ = false;
+ first_start_pos_ = 0.0;
+ error_ = false;
+ current_ = 0.0;
+ first_start_pos_ = 0.0;
+}
+
+void HallEffectAndPositionZeroingEstimator::TriggerError() {
+ if (!error_) {
+ VLOG(1) << "Manually triggered zeroing error.\n";
+ error_ = true;
+ }
+}
+
+void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
+ const HallEffectAndPosition &info) {
+ // If we have a new posedge.
+ if (!info.current()) {
+ if (last_hall_) {
+ min_low_position_ = max_low_position_ = info.encoder();
+ } else {
+ min_low_position_ = ::std::min(min_low_position_, info.encoder());
+ max_low_position_ = ::std::max(max_low_position_, info.encoder());
+ }
+ }
+ last_hall_ = info.current();
+}
+
+void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
+ const HallEffectAndPosition &info) {
+ // We want to make sure that we encounter at least one posedge while zeroing.
+ // So we take the posedge count from the first sample after reset and wait for
+ // that count to change and for the hall effect to stay high before we
+ // consider ourselves zeroed.
+ if (!initialized_) {
+ last_used_posedge_count_ = info.posedge_count();
+ initialized_ = true;
+ last_hall_ = info.current();
+ }
+
+ StoreEncoderMaxAndMin(info);
+
+ if (info.current()) {
+ cycles_high_++;
+ } else {
+ cycles_high_ = 0;
+ last_used_posedge_count_ = info.posedge_count();
+ }
+
+ high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
+
+ bool moving_backward = false;
+ if (constants_.zeroing_move_direction) {
+ moving_backward = info.encoder() > min_low_position_;
+ } else {
+ moving_backward = info.encoder() < max_low_position_;
+ }
+
+ // If there are no posedges to use or we don't have enough samples yet to
+ // have a well-filtered starting position then we use the filtered value as
+ // our best guess.
+ if (last_used_posedge_count_ != info.posedge_count() && high_long_enough_ &&
+ moving_backward) {
+ // Note the offset and the current posedge count so that we only run this
+ // logic once per posedge. That should be more resilient to corrupted
+ // intermediate data.
+ offset_ = -info.posedge_value();
+ if (constants_.zeroing_move_direction) {
+ offset_ += constants_.lower_hall_position;
+ } else {
+ offset_ += constants_.upper_hall_position;
+ }
+ last_used_posedge_count_ = info.posedge_count();
+
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = offset_;
+ VLOG(2) << "latching start position" << first_start_pos_;
+ }
+
+ // Now that we have an accurate starting position we can consider ourselves
+ // zeroed.
+ zeroed_ = true;
+ }
+
+ position_ = info.encoder() - offset_;
+}
+
+flatbuffers::Offset<HallEffectAndPositionZeroingEstimator::State>
+HallEffectAndPositionZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_encoder(position_);
+ builder.add_high_long_enough(high_long_enough_);
+ builder.add_offset(offset_);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/hall_effect_and_position.h b/frc971/zeroing/hall_effect_and_position.h
new file mode 100644
index 0000000..f03a442
--- /dev/null
+++ b/frc971/zeroing/hall_effect_and_position.h
@@ -0,0 +1,90 @@
+#ifndef FRC971_ZEROING_HALL_EFFECT_AND_POSITION_H_
+#define FRC971_ZEROING_HALL_EFFECT_AND_POSITION_H_
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Estimates the position with an incremental encoder and a hall effect sensor.
+class HallEffectAndPositionZeroingEstimator
+ : public ZeroingEstimator<HallEffectAndPosition,
+ constants::HallEffectZeroingConstants,
+ HallEffectAndPositionEstimatorState> {
+ public:
+ explicit HallEffectAndPositionZeroingEstimator(
+ const ZeroingConstants &constants);
+
+ // Update the internal logic with the next sensor values.
+ void UpdateEstimate(const Position &info) override;
+
+ // Reset the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError() override;
+
+ bool error() const override { return error_; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ bool offset_ready() const override { return zeroed_; }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ // Sets the minimum and maximum posedge position values.
+ void StoreEncoderMaxAndMin(const HallEffectAndPosition &info);
+
+ // The zeroing constants used to describe the configuration of the system.
+ const ZeroingConstants constants_;
+
+ // The estimated state of the hall effect.
+ double current_ = 0.0;
+ // The estimated position.
+ double position_ = 0.0;
+ // The smallest and largest positions of the last set of encoder positions
+ // while the hall effect was low.
+ double min_low_position_;
+ double max_low_position_;
+ // If we've seen the hall effect high for enough times without going low, then
+ // we can be sure it isn't a false positive.
+ bool high_long_enough_;
+ size_t cycles_high_;
+
+ bool last_hall_ = false;
+
+ // The estimated starting position of the mechanism. We also call this the
+ // 'offset' in some contexts.
+ double offset_;
+ // Flag for triggering logic that takes note of the current posedge count
+ // after a reset. See `last_used_posedge_count_'.
+ bool initialized_;
+ // After a reset we keep track of the posedge count with this. Only after the
+ // posedge count changes (i.e. increments at least once or wraps around) will
+ // we consider the mechanism zeroed. We also use this to store the most recent
+ // `HallEffectAndPosition::posedge_count' value when the start position
+ // was calculated. It helps us calculate the start position only on posedges
+ // to reject corrupted intermediate data.
+ int32_t last_used_posedge_count_;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_ = false;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_HALL_EFFECT_AND_POSITION_H_
diff --git a/frc971/zeroing/hall_effect_and_position_test.cc b/frc971/zeroing/hall_effect_and_position_test.cc
new file mode 100644
index 0000000..c9ddf65
--- /dev/null
+++ b/frc971/zeroing/hall_effect_and_position_test.cc
@@ -0,0 +1,135 @@
+#include "frc971/zeroing/hall_effect_and_position.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+class HallEffectAndPositionZeroingTest : public ZeroingTest {
+ protected:
+ // The starting position of the system.
+ static constexpr double kStartPosition = 2.0;
+
+ HallEffectAndPositionZeroingTest()
+ : constants_(MakeConstants()), sim_(constants_.index_difference) {
+ // Start the system out at the starting position.
+ sim_.InitializeHallEffectAndPosition(kStartPosition,
+ constants_.lower_hall_position,
+ constants_.upper_hall_position);
+ }
+
+ void MoveTo(PositionSensorSimulator *simulator,
+ HallEffectAndPositionZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
+ }
+
+ // Returns a reasonable set of test constants.
+ static constants::HallEffectZeroingConstants MakeConstants() {
+ constants::HallEffectZeroingConstants constants;
+ constants.lower_hall_position = 0.25;
+ constants.upper_hall_position = 0.75;
+ constants.index_difference = 1.0;
+ constants.hall_trigger_zeroing_length = 2;
+ constants.zeroing_move_direction = false;
+ return constants;
+ }
+
+ // Constants, and the simulation using them.
+ const constants::HallEffectZeroingConstants constants_;
+ PositionSensorSimulator sim_;
+};
+
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(HallEffectAndPositionZeroingTest, TestHallEffectZeroing) {
+ HallEffectAndPositionZeroingEstimator estimator(constants_);
+
+ // Should not be zeroed when we stand still.
+ for (int i = 0; i < 300; ++i) {
+ MoveTo(&sim_, &estimator, kStartPosition);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim_, &estimator, 1.9);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move to where the hall effect is triggered and make sure it becomes zeroed.
+ MoveTo(&sim_, &estimator, 1.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 1.5);
+ ASSERT_TRUE(estimator.zeroed());
+
+ // Check that the offset is calculated correctly. We should expect to read
+ // 0.5. Since the encoder is reading -0.5 right now, the offset needs to be
+ // 1.
+ EXPECT_DOUBLE_EQ(1.0, estimator.offset());
+
+ // Make sure triggering errors works.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Ensure resetting resets the state of the estimator.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+}
+
+// Tests that we don't zero on a too short pulse.
+TEST_F(HallEffectAndPositionZeroingTest, TestTooShortPulse) {
+ HallEffectAndPositionZeroingEstimator estimator(constants_);
+
+ // Trigger for 1 cycle.
+ MoveTo(&sim_, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+}
+
+// Tests that we don't zero when we go the wrong direction.
+TEST_F(HallEffectAndPositionZeroingTest, TestWrongDirectionNoZero) {
+ HallEffectAndPositionZeroingEstimator estimator(constants_);
+
+ // Pass through the sensor, lingering long enough that we should zero.
+ MoveTo(&sim_, &estimator, 0.0);
+ ASSERT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.4);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.6);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.7);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.9);
+ EXPECT_FALSE(estimator.zeroed());
+}
+
+// Make sure we don't zero if we start in the hall effect's range.
+TEST_F(HallEffectAndPositionZeroingTest, TestStartingOnNoZero) {
+ HallEffectAndPositionZeroingEstimator estimator(constants_);
+ MoveTo(&sim_, &estimator, 0.5);
+ estimator.Reset();
+
+ // Stay on the hall effect. We shouldn't zero.
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.5);
+ EXPECT_FALSE(estimator.zeroed());
+
+ // Verify moving off the hall still doesn't zero us.
+ MoveTo(&sim_, &estimator, 0.0);
+ EXPECT_FALSE(estimator.zeroed());
+ MoveTo(&sim_, &estimator, 0.0);
+ EXPECT_FALSE(estimator.zeroed());
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/pot_and_absolute_encoder.cc b/frc971/zeroing/pot_and_absolute_encoder.cc
new file mode 100644
index 0000000..200f399
--- /dev/null
+++ b/frc971/zeroing/pot_and_absolute_encoder.cc
@@ -0,0 +1,196 @@
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+PotAndAbsoluteEncoderZeroingEstimator::PotAndAbsoluteEncoderZeroingEstimator(
+ const constants::PotAndAbsoluteEncoderZeroingConstants &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 PotAndAbsoluteEncoderZeroingEstimator::Reset() {
+ first_offset_ = 0.0;
+ pot_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 pot 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 PotAndAbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const PotAndAbsolutePosition &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_ = pot_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;
+ }
+
+ // Now compute the offset between the pot and relative encoder.
+ if (offset_samples_.size() < constants_.average_filter_size) {
+ offset_samples_.push_back(sample.pot - sample.encoder);
+ } else {
+ offset_samples_[samples_idx_] = sample.pot - sample.encoder;
+ }
+
+ // Drop the oldest sample when we run this function the next time around.
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ pot_relative_encoder_offset_ =
+ ::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
+ offset_samples_.size();
+
+ offset_ = UnWrap(sample.encoder + pot_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))));
+
+ 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_ = pot_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
+}
+
+flatbuffers::Offset<PotAndAbsoluteEncoderZeroingEstimator::State>
+PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_pot_position(filtered_position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/pot_and_absolute_encoder.h b/frc971/zeroing/pot_and_absolute_encoder.h
new file mode 100644
index 0000000..133eaf5
--- /dev/null
+++ b/frc971/zeroing/pot_and_absolute_encoder.h
@@ -0,0 +1,100 @@
+#ifndef FRC971_ZEROING_POT_AND_ABSOLUTE_ENCODER_H_
+#define FRC971_ZEROING_POT_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 a potentiometer.
+class PotAndAbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<PotAndAbsolutePosition,
+ constants::PotAndAbsoluteEncoderZeroingConstants,
+ PotAndAbsoluteEncoderEstimatorState> {
+ public:
+ explicit PotAndAbsoluteEncoderZeroingEstimator(
+ const constants::PotAndAbsoluteEncoderZeroingConstants &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 PotAndAbsolutePosition &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 PotAndAbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()),
+ pot(position_buffer.pot()) {}
+ double absolute_encoder;
+ double encoder;
+ double pot;
+ };
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::PotAndAbsoluteEncoderZeroingConstants 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_;
+ // Offset between the Pot and Relative encoder position.
+ ::std::vector<double> offset_samples_;
+
+ MoveDetector<PositionStruct, PotAndAbsolutePosition> move_detector_;
+
+ // Estimated offset between the pot and relative encoder.
+ double pot_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_POT_AND_ABSOLUTE_ENCODER_H_
diff --git a/frc971/zeroing/pot_and_absolute_encoder_test.cc b/frc971/zeroing/pot_and_absolute_encoder_test.cc
new file mode 100644
index 0000000..ba89834
--- /dev/null
+++ b/frc971/zeroing/pot_and_absolute_encoder_test.cc
@@ -0,0 +1,143 @@
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::PotAndAbsoluteEncoderZeroingConstants;
+
+class PotAndAbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ PotAndAbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<PotAndAbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(PotAndAbsoluteEncoderZeroingTest,
+ TestPotAndAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ PotAndAbsoluteEncoderZeroingConstants 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);
+
+ PotAndAbsoluteEncoderZeroingEstimator 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(PotAndAbsoluteEncoderZeroingTest,
+ TestPotAndAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ PotAndAbsoluteEncoderZeroingConstants 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);
+
+ PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreatePotAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(
+ *flatbuffers::GetRoot<PotAndAbsolutePosition>(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(PotAndAbsoluteEncoderZeroingTest,
+ TestPotAndAbsoluteEncoderZeroingWithMovement) {
+ 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;
+
+ PotAndAbsoluteEncoderZeroingConstants 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);
+
+ PotAndAbsoluteEncoderZeroingEstimator 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(PotAndAbsoluteEncoderZeroingTest,
+ TestPotAndAbsoluteEncoderZeroingWithNaN) {
+ PotAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ FBB fbb;
+ fbb.Finish(CreatePotAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ const auto sensor_values =
+ flatbuffers::GetRoot<PotAndAbsolutePosition>(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());
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/pot_and_index.cc b/frc971/zeroing/pot_and_index.cc
new file mode 100644
index 0000000..7c713d9
--- /dev/null
+++ b/frc971/zeroing/pot_and_index.cc
@@ -0,0 +1,135 @@
+#include "frc971/zeroing/pot_and_index.h"
+
+#include <cmath>
+
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace zeroing {
+
+PotAndIndexPulseZeroingEstimator::PotAndIndexPulseZeroingEstimator(
+ const constants::PotAndIndexPulseZeroingConstants &constants)
+ : constants_(constants) {
+ start_pos_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void PotAndIndexPulseZeroingEstimator::Reset() {
+ samples_idx_ = 0;
+ offset_ = 0;
+ start_pos_samples_.clear();
+ zeroed_ = false;
+ wait_for_index_pulse_ = true;
+ last_used_index_pulse_count_ = 0;
+ error_ = false;
+}
+
+void PotAndIndexPulseZeroingEstimator::TriggerError() {
+ if (!error_) {
+ VLOG(1) << "Manually triggered zeroing error.";
+ error_ = true;
+ }
+}
+
+double PotAndIndexPulseZeroingEstimator::CalculateStartPosition(
+ double start_average, double latched_encoder) const {
+ // We calculate an aproximation of the value of the last index position.
+ // Also account for index pulses not lining up with integer multiples of the
+ // index_diff.
+ double index_pos =
+ start_average + latched_encoder - constants_.measured_index_position;
+ // We round index_pos to the closest valid value of the index.
+ double accurate_index_pos = (round(index_pos / constants_.index_difference)) *
+ constants_.index_difference;
+ // Now we reverse the first calculation to get the accurate start position.
+ return accurate_index_pos - latched_encoder +
+ constants_.measured_index_position;
+}
+
+void PotAndIndexPulseZeroingEstimator::UpdateEstimate(
+ const PotAndIndexPosition &info) {
+ // We want to make sure that we encounter at least one index pulse while
+ // zeroing. So we take the index pulse count from the first sample after
+ // reset and wait for that count to change before we consider ourselves
+ // zeroed.
+ if (wait_for_index_pulse_) {
+ last_used_index_pulse_count_ = info.index_pulses();
+ wait_for_index_pulse_ = false;
+ }
+
+ if (start_pos_samples_.size() < constants_.average_filter_size) {
+ start_pos_samples_.push_back(info.pot() - info.encoder());
+ } else {
+ start_pos_samples_[samples_idx_] = info.pot() - info.encoder();
+ }
+
+ // Drop the oldest sample when we run this function the next time around.
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ double sample_sum = 0.0;
+
+ for (size_t i = 0; i < start_pos_samples_.size(); ++i) {
+ sample_sum += start_pos_samples_[i];
+ }
+
+ // Calculates the average of the starting position.
+ double start_average = sample_sum / start_pos_samples_.size();
+
+ // If there are no index pulses to use or we don't have enough samples yet to
+ // have a well-filtered starting position then we use the filtered value as
+ // our best guess.
+ if (!zeroed_ &&
+ (info.index_pulses() == last_used_index_pulse_count_ || !offset_ready())) {
+ offset_ = start_average;
+ } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses()) {
+ // Note the accurate start position and the current index pulse count so
+ // that we only run this logic once per index pulse. That should be more
+ // resilient to corrupted intermediate data.
+ offset_ = CalculateStartPosition(start_average, info.latched_encoder());
+ last_used_index_pulse_count_ = info.index_pulses();
+
+ // TODO(austin): Reject encoder positions which have x% error rather than
+ // rounding to the closest index pulse.
+
+ // Save the first starting position.
+ if (!zeroed_) {
+ first_start_pos_ = offset_;
+ VLOG(2) << "latching start position" << first_start_pos_;
+ }
+
+ // Now that we have an accurate starting position we can consider ourselves
+ // zeroed.
+ zeroed_ = true;
+ // Throw an error if first_start_pos is bigger/smaller than
+ // constants_.allowable_encoder_error * index_diff + start_pos.
+ if (::std::abs(first_start_pos_ - offset_) >
+ constants_.allowable_encoder_error * constants_.index_difference) {
+ if (!error_) {
+ VLOG(1)
+ << "Encoder ticks out of range since last index pulse. first start "
+ "position: "
+ << first_start_pos_ << " recent starting position: " << offset_
+ << ", allowable error: "
+ << constants_.allowable_encoder_error * constants_.index_difference;
+ error_ = true;
+ }
+ }
+ }
+
+ position_ = offset_ + info.encoder();
+ filtered_position_ = start_average + info.encoder();
+}
+
+flatbuffers::Offset<PotAndIndexPulseZeroingEstimator::State>
+PotAndIndexPulseZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_pot_position(filtered_position_);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/pot_and_index.h b/frc971/zeroing/pot_and_index.h
new file mode 100644
index 0000000..473c674
--- /dev/null
+++ b/frc971/zeroing/pot_and_index.h
@@ -0,0 +1,103 @@
+#ifndef FRC971_ZEROING_POT_AND_INDEX_H_
+#define FRC971_ZEROING_POT_AND_INDEX_H_
+
+#include <vector>
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Estimates the position with an incremental encoder with an index pulse and a
+// potentiometer.
+class PotAndIndexPulseZeroingEstimator
+ : public ZeroingEstimator<PotAndIndexPosition,
+ constants::PotAndIndexPulseZeroingConstants,
+ EstimatorState> {
+ public:
+ explicit PotAndIndexPulseZeroingEstimator(
+ const constants::PotAndIndexPulseZeroingConstants &constants);
+
+ // Update the internal logic with the next sensor values.
+ void UpdateEstimate(const PotAndIndexPosition &info) override;
+
+ // Reset the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError() override;
+
+ bool error() const override { return error_; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ // Returns a number between 0 and 1 that represents the percentage of the
+ // samples being used in the moving average filter. A value of 0.0 means that
+ // no samples are being used. A value of 1.0 means that the filter is using
+ // as many samples as it has room for. For example, after a Reset() this
+ // value returns 0.0. As more samples get added with UpdateEstimate(...) the
+ // return value starts increasing to 1.0.
+ double offset_ratio_ready() const {
+ return start_pos_samples_.size() /
+ static_cast<double>(constants_.average_filter_size);
+ }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const override {
+ return start_pos_samples_.size() == constants_.average_filter_size;
+ }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ // This function calculates the start position given the internal state and
+ // the provided `latched_encoder' value.
+ double CalculateStartPosition(double start_average,
+ double latched_encoder) const;
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::PotAndIndexPulseZeroingConstants constants_;
+
+ // The estimated position.
+ double position_;
+ // The unzeroed filtered position.
+ double filtered_position_ = 0.0;
+ // The next position in 'start_pos_samples_' to be used to store the next
+ // sample.
+ int samples_idx_;
+ // Last 'max_sample_count_' samples for start positions.
+ std::vector<double> start_pos_samples_;
+ // The estimated starting position of the mechanism. We also call this the
+ // 'offset' in some contexts.
+ double offset_;
+ // Flag for triggering logic that takes note of the current index pulse count
+ // after a reset. See `last_used_index_pulse_count_'.
+ bool wait_for_index_pulse_;
+ // After a reset we keep track of the index pulse count with this. Only after
+ // the index pulse count changes (i.e. increments at least once or wraps
+ // around) will we consider the mechanism zeroed. We also use this to store
+ // the most recent `PotAndIndexPosition::index_pulses' value when the start
+ // position was calculated. It helps us calculate the start position only on
+ // index pulses to reject corrupted intermediate data.
+ uint32_t last_used_index_pulse_count_;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_;
+ // Stores the position "start_pos" variable the first time the program
+ // is zeroed.
+ double first_start_pos_;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_POT_AND_INDEX_H_
diff --git a/frc971/zeroing/pot_and_index_test.cc b/frc971/zeroing/pot_and_index_test.cc
new file mode 100644
index 0000000..b2b00ce
--- /dev/null
+++ b/frc971/zeroing/pot_and_index_test.cc
@@ -0,0 +1,283 @@
+#include "frc971/zeroing/pot_and_index.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::PotAndIndexPulseZeroingConstants;
+
+class PotAndIndexZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ PotAndIndexPulseZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<PotAndIndexPosition>(&fbb));
+ }
+};
+
+TEST_F(PotAndIndexZeroingTest, TestMovingAverageFilter) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.6 * index_diff, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ // The zeroing code is supposed to perform some filtering on the difference
+ // between the potentiometer value and the encoder value. We assume that 300
+ // samples are sufficient to have updated the filter.
+ for (int i = 0; i < 300; i++) {
+ MoveTo(&sim, &estimator, 3.3 * index_diff);
+ }
+ ASSERT_NEAR(3.3 * index_diff, GetEstimatorPosition(&estimator),
+ kAcceptableUnzeroedError * index_diff);
+
+ for (int i = 0; i < 300; i++) {
+ MoveTo(&sim, &estimator, 3.9 * index_diff);
+ }
+ ASSERT_NEAR(3.9 * index_diff, GetEstimatorPosition(&estimator),
+ kAcceptableUnzeroedError * index_diff);
+}
+
+TEST_F(PotAndIndexZeroingTest, NotZeroedBeforeEnoughSamplesCollected) {
+ double index_diff = 0.5;
+ double position = 3.6 * index_diff;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(position, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ // Make sure that the zeroing code does not consider itself zeroed until we
+ // collect a good amount of samples. In this case we're waiting until the
+ // moving average filter is full.
+ for (unsigned int i = 0; i < kSampleSize - 1; i++) {
+ MoveTo(&sim, &estimator, position += index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, position);
+ ASSERT_TRUE(estimator.zeroed());
+}
+
+TEST_F(PotAndIndexZeroingTest, TestLotsOfMovement) {
+ double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.6, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ // The zeroing code is supposed to perform some filtering on the difference
+ // between the potentiometer value and the encoder value. We assume that 300
+ // samples are sufficient to have updated the filter.
+ for (int i = 0; i < 300; i++) {
+ MoveTo(&sim, &estimator, 3.6);
+ }
+ ASSERT_NEAR(3.6, GetEstimatorPosition(&estimator),
+ kAcceptableUnzeroedError * index_diff);
+
+ // With a single index pulse the zeroing estimator should be able to lock
+ // onto the true value of the position.
+ MoveTo(&sim, &estimator, 4.01);
+ ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 4.99);
+ ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 3.99);
+ ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 3.01);
+ ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 13.55);
+ ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
+}
+
+TEST_F(PotAndIndexZeroingTest, TestDifferentIndexDiffs) {
+ double index_diff = 0.89;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.5 * index_diff, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ // The zeroing code is supposed to perform some filtering on the difference
+ // between the potentiometer value and the encoder value. We assume that 300
+ // samples are sufficient to have updated the filter.
+ for (int i = 0; i < 300; i++) {
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ }
+ ASSERT_NEAR(3.5 * index_diff, GetEstimatorPosition(&estimator),
+ kAcceptableUnzeroedError * index_diff);
+
+ // With a single index pulse the zeroing estimator should be able to lock
+ // onto the true value of the position.
+ MoveTo(&sim, &estimator, 4.01);
+ ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 4.99);
+ ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 3.99);
+ ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 3.01);
+ ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
+
+ MoveTo(&sim, &estimator, 13.55);
+ ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
+}
+
+TEST_F(PotAndIndexZeroingTest, TestPercentage) {
+ double index_diff = 0.89;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.5 * index_diff, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ for (unsigned int i = 0; i < kSampleSize / 2; i++) {
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ }
+ ASSERT_NEAR(0.5, estimator.offset_ratio_ready(), 0.001);
+ ASSERT_FALSE(estimator.offset_ready());
+
+ for (unsigned int i = 0; i < kSampleSize / 2; i++) {
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ }
+ ASSERT_NEAR(1.0, estimator.offset_ratio_ready(), 0.001);
+ ASSERT_TRUE(estimator.offset_ready());
+}
+
+TEST_F(PotAndIndexZeroingTest, TestOffset) {
+ double index_diff = 0.89;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.1 * index_diff, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ MoveTo(&sim, &estimator, 3.1 * index_diff);
+
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 5.0 * index_diff);
+ }
+
+ ASSERT_NEAR(3.1 * index_diff, estimator.offset(), 0.001);
+}
+
+TEST_F(PotAndIndexZeroingTest, WaitForIndexPulseAfterReset) {
+ double index_diff = 0.6;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.1 * index_diff, index_diff / 3.0);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+
+ // Make sure to fill up the averaging filter with samples.
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 3.1 * 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, 4.5 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+
+ // Reset the zeroing logic and supply a bunch of samples within the current
+ // index segment.
+ estimator.Reset();
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 4.2 * index_diff);
+ }
+
+ // Make sure we're not zeroed until we hit an index pulse.
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Trigger another index pulse; we should be zeroed again.
+ MoveTo(&sim, &estimator, 3.1 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+}
+
+TEST_F(PotAndIndexZeroingTest, 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);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
+
+ // 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, GetEstimatorPosition(&estimator));
+
+ // 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, GetEstimatorPosition(&estimator));
+}
+
+TEST_F(PotAndIndexZeroingTest, BasicErrorAPITest) {
+ const double index_diff = 1.0;
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ kSampleSize, index_diff, 0.0, kIndexErrorFraction});
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
+
+ // Perform a simple move and make sure that no error occured.
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ ASSERT_FALSE(estimator.error());
+
+ // Trigger an error and make sure it's reported.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Make sure that it can recover after a reset.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.error());
+ MoveTo(&sim, &estimator, 4.5 * index_diff);
+ MoveTo(&sim, &estimator, 5.5 * index_diff);
+ ASSERT_FALSE(estimator.error());
+}
+
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(PotAndIndexZeroingTest, TestIndexOffsetError) {
+ const double index_diff = 0.8;
+ const double known_index_pos = 2 * index_diff;
+ const size_t sample_size = 30;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
+ PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
+ sample_size, index_diff, known_index_pos, kIndexErrorFraction});
+
+ for (size_t i = 0; i < sample_size; i++) {
+ MoveTo(&sim, &estimator, 13 * index_diff);
+ }
+ MoveTo(&sim, &estimator, 8 * index_diff);
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+ sim.Initialize(9.0 * index_diff + 0.31 * index_diff, index_diff / 3.0,
+ known_index_pos);
+ MoveTo(&sim, &estimator, 9 * index_diff);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.error());
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/pulse_index.cc b/frc971/zeroing/pulse_index.cc
new file mode 100644
index 0000000..c0831e8
--- /dev/null
+++ b/frc971/zeroing/pulse_index.cc
@@ -0,0 +1,116 @@
+#include "frc971/zeroing/pulse_index.h"
+
+#include <cmath>
+#include <limits>
+
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace zeroing {
+
+void PulseIndexZeroingEstimator::Reset() {
+ max_index_position_ = ::std::numeric_limits<double>::lowest();
+ min_index_position_ = ::std::numeric_limits<double>::max();
+ offset_ = 0;
+ last_used_index_pulse_count_ = 0;
+ zeroed_ = false;
+ error_ = false;
+}
+
+void PulseIndexZeroingEstimator::StoreIndexPulseMaxAndMin(
+ const IndexPosition &info) {
+ // If we have a new index pulse.
+ if (last_used_index_pulse_count_ != info.index_pulses()) {
+ // If the latest pulses's position is outside the range we've currently
+ // seen, record it appropriately.
+ if (info.latched_encoder() > max_index_position_) {
+ max_index_position_ = info.latched_encoder();
+ }
+ if (info.latched_encoder() < min_index_position_) {
+ min_index_position_ = info.latched_encoder();
+ }
+ last_used_index_pulse_count_ = info.index_pulses();
+ }
+}
+
+int PulseIndexZeroingEstimator::IndexPulseCount() const {
+ if (min_index_position_ > max_index_position_) {
+ // This condition means we haven't seen a pulse yet.
+ return 0;
+ }
+
+ // Calculate the number of pulses encountered so far.
+ return 1 + static_cast<int>(
+ ::std::round((max_index_position_ - min_index_position_) /
+ constants_.index_difference));
+}
+
+void PulseIndexZeroingEstimator::UpdateEstimate(const IndexPosition &info) {
+ StoreIndexPulseMaxAndMin(info);
+ const int index_pulse_count = IndexPulseCount();
+ if (index_pulse_count > constants_.index_pulse_count) {
+ if (!error_) {
+ VLOG(1) << "Got more index pulses than expected. Got "
+ << index_pulse_count << " expected "
+ << constants_.index_pulse_count;
+ error_ = true;
+ }
+ }
+
+ // TODO(austin): Detect if the encoder or index pulse is unplugged.
+ // TODO(austin): Detect missing counts.
+
+ if (index_pulse_count == constants_.index_pulse_count && !zeroed_) {
+ offset_ = constants_.measured_index_position -
+ constants_.known_index_pulse * constants_.index_difference -
+ min_index_position_;
+ zeroed_ = true;
+ } else if (zeroed_ && !error_) {
+ // Detect whether the index pulse is somewhere other than where we expect
+ // it to be. First we compute the position of the most recent index pulse.
+ double index_pulse_distance =
+ info.latched_encoder() + offset_ - constants_.measured_index_position;
+ // Second we compute the position of the index pulse in terms of
+ // the index difference. I.e. if this index pulse is two pulses away from
+ // the index pulse that we know about then this number should be positive
+ // or negative two.
+ double relative_distance =
+ index_pulse_distance / constants_.index_difference;
+ // Now we compute how far away the measured index pulse is from the
+ // expected index pulse.
+ double error = relative_distance - ::std::round(relative_distance);
+ // This lets us check if the index pulse is within an acceptable error
+ // margin of where we expected it to be.
+ if (::std::abs(error) > constants_.allowable_encoder_error) {
+ VLOG(1)
+ << "Encoder ticks out of range since last index pulse. known index "
+ "pulse: "
+ << constants_.measured_index_position << ", expected index pulse: "
+ << round(relative_distance) * constants_.index_difference +
+ constants_.measured_index_position
+ << ", actual index pulse: " << info.latched_encoder() + offset_
+ << ", "
+ "allowable error: "
+ << constants_.allowable_encoder_error * constants_.index_difference;
+ error_ = true;
+ }
+ }
+
+ position_ = info.encoder() + offset_;
+}
+
+flatbuffers::Offset<PulseIndexZeroingEstimator::State>
+PulseIndexZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_min_index_position(min_index_position_);
+ builder.add_max_index_position(max_index_position_);
+ builder.add_index_pulses_seen(IndexPulseCount());
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/pulse_index.h b/frc971/zeroing/pulse_index.h
new file mode 100644
index 0000000..4bcf210
--- /dev/null
+++ b/frc971/zeroing/pulse_index.h
@@ -0,0 +1,84 @@
+#ifndef FRC971_ZEROING_PULSE_INDEX_H_
+#define FRC971_ZEROING_PULSE_INDEX_H_
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Zeros by seeing all the index pulses in the range of motion of the mechanism
+// and using that to figure out which index pulse is which.
+class PulseIndexZeroingEstimator
+ : public ZeroingEstimator<IndexPosition,
+ constants::EncoderPlusIndexZeroingConstants,
+ IndexEstimatorState> {
+ public:
+ explicit PulseIndexZeroingEstimator(const ZeroingConstants &constants)
+ : constants_(constants) {
+ Reset();
+ }
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ bool zeroed() const override { return zeroed_; }
+
+ // It's as ready as it'll ever be...
+ bool offset_ready() const override { return true; }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Updates the internal logic with the next sensor values.
+ void UpdateEstimate(const IndexPosition &info) override;
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ void TriggerError() override { error_ = true; }
+
+ private:
+ // Returns the current real position using the relative encoder offset.
+ double CalculateCurrentPosition(const IndexPosition &info);
+
+ // Sets the minimum and maximum index pulse position values.
+ void StoreIndexPulseMaxAndMin(const IndexPosition &info);
+
+ // Returns the number of index pulses we should have seen so far.
+ int IndexPulseCount() const;
+
+ // Contains the physical constants describing the system.
+ const ZeroingConstants constants_;
+
+ // The smallest position of all the index pulses.
+ double min_index_position_;
+ // The largest position of all the index pulses.
+ double max_index_position_;
+
+ // The estimated starting position of the mechanism.
+ double offset_;
+ // After a reset we keep track of the index pulse count with this. Only after
+ // the index pulse count changes (i.e. increments at least once or wraps
+ // around) will we consider the mechanism zeroed. We also use this to store
+ // the most recent `PotAndIndexPosition::index_pulses' value when the start
+ // position was calculated. It helps us calculate the start position only on
+ // index pulses to reject corrupted intermediate data.
+ uint32_t last_used_index_pulse_count_;
+
+ // True if we are fully zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+
+ // The estimated position.
+ double position_;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_PULSE_INDEX_H_
diff --git a/frc971/zeroing/pulse_index_test.cc b/frc971/zeroing/pulse_index_test.cc
new file mode 100644
index 0000000..bd257f7
--- /dev/null
+++ b/frc971/zeroing/pulse_index_test.cc
@@ -0,0 +1,132 @@
+#include "frc971/zeroing/pulse_index.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::EncoderPlusIndexZeroingConstants;
+
+class PulseIndexZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ PulseIndexZeroingEstimator *estimator, double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<IndexPosition>(&fbb));
+ }
+};
+
+// Tests that an error is detected when the starting position changes too much.
+TEST_F(PulseIndexZeroingTest, TestRelativeEncoderZeroing) {
+ EncoderPlusIndexZeroingConstants constants;
+ constants.index_pulse_count = 3;
+ constants.index_difference = 10.0;
+ constants.measured_index_position = 20.0;
+ constants.known_index_pulse = 1;
+ constants.allowable_encoder_error = 0.01;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos = 2.5 * constants.index_difference;
+
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position);
+
+ PulseIndexZeroingEstimator estimator(constants);
+
+ // Should not be zeroed when we stand still.
+ for (int i = 0; i < 300; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ // Move to 1.5 constants.index_difference and we should still not be zeroed.
+ MoveTo(&sim, &estimator, 1.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move to 0.5 constants.index_difference and we should still not be zeroed.
+ MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move back to 1.5 constants.index_difference and we should still not be
+ // zeroed.
+ MoveTo(&sim, &estimator, 1.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move back to 2.5 constants.index_difference and we should still not be
+ // zeroed.
+ MoveTo(&sim, &estimator, 2.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move back to 3.5 constants.index_difference and we should now be zeroed.
+ MoveTo(&sim, &estimator, 3.5 * constants.index_difference);
+ ASSERT_TRUE(estimator.zeroed());
+
+ ASSERT_DOUBLE_EQ(start_pos, estimator.offset());
+ ASSERT_DOUBLE_EQ(3.5 * constants.index_difference,
+ GetEstimatorPosition(&estimator));
+
+ MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
+ ASSERT_DOUBLE_EQ(0.5 * constants.index_difference,
+ GetEstimatorPosition(&estimator));
+}
+
+// Tests that we can detect when an index pulse occurs where we didn't expect
+// it to for the PulseIndexZeroingEstimator.
+TEST_F(PulseIndexZeroingTest, TestRelativeEncoderSlipping) {
+ EncoderPlusIndexZeroingConstants constants;
+ constants.index_pulse_count = 3;
+ constants.index_difference = 10.0;
+ constants.measured_index_position = 20.0;
+ constants.known_index_pulse = 1;
+ constants.allowable_encoder_error = 0.05;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos =
+ constants.measured_index_position + 0.5 * constants.index_difference;
+
+ for (double direction : {1.0, -1.0}) {
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position);
+
+ PulseIndexZeroingEstimator estimator(constants);
+
+ // Zero the estimator.
+ MoveTo(&sim, &estimator, start_pos - 1 * constants.index_difference);
+ MoveTo(
+ &sim, &estimator,
+ start_pos - constants.index_pulse_count * constants.index_difference);
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+
+ // We have a 5% allowable error so we slip a little bit each time and make
+ // sure that the index pulses are still accepted.
+ for (double error = 0.00;
+ ::std::abs(error) < constants.allowable_encoder_error;
+ error += 0.01 * direction) {
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position +
+ error * constants.index_difference);
+ MoveTo(&sim, &estimator, start_pos - constants.index_difference);
+ EXPECT_FALSE(estimator.error());
+ }
+
+ // As soon as we hit cross the error margin, we should trigger an error.
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position +
+ constants.allowable_encoder_error * 1.1 *
+ constants.index_difference * direction);
+ MoveTo(&sim, &estimator, start_pos - constants.index_difference);
+ ASSERT_TRUE(estimator.error());
+ }
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/relative_encoder_test.cc b/frc971/zeroing/relative_encoder_test.cc
new file mode 100644
index 0000000..fd86fb3
--- /dev/null
+++ b/frc971/zeroing/relative_encoder_test.cc
@@ -0,0 +1,40 @@
+#include "frc971/zeroing/zeroing.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+class RelativeEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ RelativeEncoderZeroingEstimator *estimator, double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<RelativePosition>(&fbb));
+ }
+};
+
+TEST_F(RelativeEncoderZeroingTest, TestRelativeEncoderZeroingWithoutMovement) {
+ PositionSensorSimulator sim(1.0);
+ RelativeEncoderZeroingEstimator estimator;
+
+ sim.InitializeRelativeEncoder();
+
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_TRUE(estimator.offset_ready());
+ EXPECT_DOUBLE_EQ(estimator.offset(), 0.0);
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.0);
+
+ MoveTo(&sim, &estimator, 0.1);
+
+ EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.1);
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
deleted file mode 100644
index f0aab23..0000000
--- a/frc971/zeroing/zeroing.cc
+++ /dev/null
@@ -1,706 +0,0 @@
-#include "frc971/zeroing/zeroing.h"
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-#include <numeric>
-#include <vector>
-
-#include "frc971/zeroing/wrap.h"
-
-#include "flatbuffers/flatbuffers.h"
-#include "glog/logging.h"
-
-namespace frc971 {
-namespace zeroing {
-
-PotAndIndexPulseZeroingEstimator::PotAndIndexPulseZeroingEstimator(
- const constants::PotAndIndexPulseZeroingConstants &constants)
- : constants_(constants) {
- start_pos_samples_.reserve(constants_.average_filter_size);
- Reset();
-}
-
-void PotAndIndexPulseZeroingEstimator::Reset() {
- samples_idx_ = 0;
- offset_ = 0;
- start_pos_samples_.clear();
- zeroed_ = false;
- wait_for_index_pulse_ = true;
- last_used_index_pulse_count_ = 0;
- error_ = false;
-}
-
-void PotAndIndexPulseZeroingEstimator::TriggerError() {
- if (!error_) {
- VLOG(1) << "Manually triggered zeroing error.";
- error_ = true;
- }
-}
-
-double PotAndIndexPulseZeroingEstimator::CalculateStartPosition(
- double start_average, double latched_encoder) const {
- // We calculate an aproximation of the value of the last index position.
- // Also account for index pulses not lining up with integer multiples of the
- // index_diff.
- double index_pos =
- start_average + latched_encoder - constants_.measured_index_position;
- // We round index_pos to the closest valid value of the index.
- double accurate_index_pos = (round(index_pos / constants_.index_difference)) *
- constants_.index_difference;
- // Now we reverse the first calculation to get the accurate start position.
- return accurate_index_pos - latched_encoder +
- constants_.measured_index_position;
-}
-
-void PotAndIndexPulseZeroingEstimator::UpdateEstimate(
- const PotAndIndexPosition &info) {
- // We want to make sure that we encounter at least one index pulse while
- // zeroing. So we take the index pulse count from the first sample after
- // reset and wait for that count to change before we consider ourselves
- // zeroed.
- if (wait_for_index_pulse_) {
- last_used_index_pulse_count_ = info.index_pulses();
- wait_for_index_pulse_ = false;
- }
-
- if (start_pos_samples_.size() < constants_.average_filter_size) {
- start_pos_samples_.push_back(info.pot() - info.encoder());
- } else {
- start_pos_samples_[samples_idx_] = info.pot() - info.encoder();
- }
-
- // Drop the oldest sample when we run this function the next time around.
- samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
-
- double sample_sum = 0.0;
-
- for (size_t i = 0; i < start_pos_samples_.size(); ++i) {
- sample_sum += start_pos_samples_[i];
- }
-
- // Calculates the average of the starting position.
- double start_average = sample_sum / start_pos_samples_.size();
-
- // If there are no index pulses to use or we don't have enough samples yet to
- // have a well-filtered starting position then we use the filtered value as
- // our best guess.
- if (!zeroed_ &&
- (info.index_pulses() == last_used_index_pulse_count_ || !offset_ready())) {
- offset_ = start_average;
- } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses()) {
- // Note the accurate start position and the current index pulse count so
- // that we only run this logic once per index pulse. That should be more
- // resilient to corrupted intermediate data.
- offset_ = CalculateStartPosition(start_average, info.latched_encoder());
- last_used_index_pulse_count_ = info.index_pulses();
-
- // TODO(austin): Reject encoder positions which have x% error rather than
- // rounding to the closest index pulse.
-
- // Save the first starting position.
- if (!zeroed_) {
- first_start_pos_ = offset_;
- VLOG(2) << "latching start position" << first_start_pos_;
- }
-
- // Now that we have an accurate starting position we can consider ourselves
- // zeroed.
- zeroed_ = true;
- // Throw an error if first_start_pos is bigger/smaller than
- // constants_.allowable_encoder_error * index_diff + start_pos.
- if (::std::abs(first_start_pos_ - offset_) >
- constants_.allowable_encoder_error * constants_.index_difference) {
- if (!error_) {
- VLOG(1)
- << "Encoder ticks out of range since last index pulse. first start "
- "position: "
- << first_start_pos_ << " recent starting position: " << offset_
- << ", allowable error: "
- << constants_.allowable_encoder_error * constants_.index_difference;
- error_ = true;
- }
- }
- }
-
- position_ = offset_ + info.encoder();
- filtered_position_ = start_average + info.encoder();
-}
-
-flatbuffers::Offset<PotAndIndexPulseZeroingEstimator::State>
-PotAndIndexPulseZeroingEstimator::GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const {
- State::Builder builder(*fbb);
- builder.add_error(error_);
- builder.add_zeroed(zeroed_);
- builder.add_position(position_);
- builder.add_pot_position(filtered_position_);
- return builder.Finish();
-}
-
-HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
- const ZeroingConstants &constants)
- : constants_(constants) {
- Reset();
-}
-
-void HallEffectAndPositionZeroingEstimator::Reset() {
- offset_ = 0.0;
- min_low_position_ = ::std::numeric_limits<double>::max();
- max_low_position_ = ::std::numeric_limits<double>::lowest();
- zeroed_ = false;
- initialized_ = false;
- last_used_posedge_count_ = 0;
- cycles_high_ = 0;
- high_long_enough_ = false;
- first_start_pos_ = 0.0;
- error_ = false;
- current_ = 0.0;
- first_start_pos_ = 0.0;
-}
-
-void HallEffectAndPositionZeroingEstimator::TriggerError() {
- if (!error_) {
- VLOG(1) << "Manually triggered zeroing error.\n";
- error_ = true;
- }
-}
-
-void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
- const HallEffectAndPosition &info) {
- // If we have a new posedge.
- if (!info.current()) {
- if (last_hall_) {
- min_low_position_ = max_low_position_ = info.encoder();
- } else {
- min_low_position_ = ::std::min(min_low_position_, info.encoder());
- max_low_position_ = ::std::max(max_low_position_, info.encoder());
- }
- }
- last_hall_ = info.current();
-}
-
-void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
- const HallEffectAndPosition &info) {
- // We want to make sure that we encounter at least one posedge while zeroing.
- // So we take the posedge count from the first sample after reset and wait for
- // that count to change and for the hall effect to stay high before we
- // consider ourselves zeroed.
- if (!initialized_) {
- last_used_posedge_count_ = info.posedge_count();
- initialized_ = true;
- last_hall_ = info.current();
- }
-
- StoreEncoderMaxAndMin(info);
-
- if (info.current()) {
- cycles_high_++;
- } else {
- cycles_high_ = 0;
- last_used_posedge_count_ = info.posedge_count();
- }
-
- high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
-
- bool moving_backward = false;
- if (constants_.zeroing_move_direction) {
- moving_backward = info.encoder() > min_low_position_;
- } else {
- moving_backward = info.encoder() < max_low_position_;
- }
-
- // If there are no posedges to use or we don't have enough samples yet to
- // have a well-filtered starting position then we use the filtered value as
- // our best guess.
- if (last_used_posedge_count_ != info.posedge_count() && high_long_enough_ &&
- moving_backward) {
- // Note the offset and the current posedge count so that we only run this
- // logic once per posedge. That should be more resilient to corrupted
- // intermediate data.
- offset_ = -info.posedge_value();
- if (constants_.zeroing_move_direction) {
- offset_ += constants_.lower_hall_position;
- } else {
- offset_ += constants_.upper_hall_position;
- }
- last_used_posedge_count_ = info.posedge_count();
-
- // Save the first starting position.
- if (!zeroed_) {
- first_start_pos_ = offset_;
- VLOG(2) << "latching start position" << first_start_pos_;
- }
-
- // Now that we have an accurate starting position we can consider ourselves
- // zeroed.
- zeroed_ = true;
- }
-
- position_ = info.encoder() - offset_;
-}
-
-flatbuffers::Offset<HallEffectAndPositionZeroingEstimator::State>
-HallEffectAndPositionZeroingEstimator::GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const {
- State::Builder builder(*fbb);
- builder.add_error(error_);
- builder.add_zeroed(zeroed_);
- builder.add_encoder(position_);
- builder.add_high_long_enough(high_long_enough_);
- builder.add_offset(offset_);
- return builder.Finish();
-}
-
-PotAndAbsoluteEncoderZeroingEstimator::PotAndAbsoluteEncoderZeroingEstimator(
- const constants::PotAndAbsoluteEncoderZeroingConstants &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 PotAndAbsoluteEncoderZeroingEstimator::Reset() {
- first_offset_ = 0.0;
- pot_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 pot 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 PotAndAbsoluteEncoderZeroingEstimator::UpdateEstimate(
- const PotAndAbsolutePosition &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_ = pot_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;
- }
-
- // Now compute the offset between the pot and relative encoder.
- if (offset_samples_.size() < constants_.average_filter_size) {
- offset_samples_.push_back(sample.pot - sample.encoder);
- } else {
- offset_samples_[samples_idx_] = sample.pot - sample.encoder;
- }
-
- // Drop the oldest sample when we run this function the next time around.
- samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
-
- pot_relative_encoder_offset_ =
- ::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
- offset_samples_.size();
-
- offset_ = UnWrap(sample.encoder + pot_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))));
-
- 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_ = pot_relative_encoder_offset_ + info.encoder();
- position_ = offset_ + info.encoder();
-}
-
-flatbuffers::Offset<PotAndAbsoluteEncoderZeroingEstimator::State>
-PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const {
- State::Builder builder(*fbb);
- builder.add_error(error_);
- builder.add_zeroed(zeroed_);
- builder.add_position(position_);
- builder.add_pot_position(filtered_position_);
- builder.add_absolute_position(filtered_absolute_encoder_);
- return builder.Finish();
-}
-
-void PulseIndexZeroingEstimator::Reset() {
- max_index_position_ = ::std::numeric_limits<double>::lowest();
- min_index_position_ = ::std::numeric_limits<double>::max();
- offset_ = 0;
- last_used_index_pulse_count_ = 0;
- zeroed_ = false;
- error_ = false;
-}
-
-void PulseIndexZeroingEstimator::StoreIndexPulseMaxAndMin(
- const IndexPosition &info) {
- // If we have a new index pulse.
- if (last_used_index_pulse_count_ != info.index_pulses()) {
- // If the latest pulses's position is outside the range we've currently
- // seen, record it appropriately.
- if (info.latched_encoder() > max_index_position_) {
- max_index_position_ = info.latched_encoder();
- }
- if (info.latched_encoder() < min_index_position_) {
- min_index_position_ = info.latched_encoder();
- }
- last_used_index_pulse_count_ = info.index_pulses();
- }
-}
-
-int PulseIndexZeroingEstimator::IndexPulseCount() const {
- if (min_index_position_ > max_index_position_) {
- // This condition means we haven't seen a pulse yet.
- return 0;
- }
-
- // Calculate the number of pulses encountered so far.
- return 1 + static_cast<int>(
- ::std::round((max_index_position_ - min_index_position_) /
- constants_.index_difference));
-}
-
-void PulseIndexZeroingEstimator::UpdateEstimate(const IndexPosition &info) {
- StoreIndexPulseMaxAndMin(info);
- const int index_pulse_count = IndexPulseCount();
- if (index_pulse_count > constants_.index_pulse_count) {
- if (!error_) {
- VLOG(1) << "Got more index pulses than expected. Got "
- << index_pulse_count << " expected "
- << constants_.index_pulse_count;
- error_ = true;
- }
- }
-
- // TODO(austin): Detect if the encoder or index pulse is unplugged.
- // TODO(austin): Detect missing counts.
-
- if (index_pulse_count == constants_.index_pulse_count && !zeroed_) {
- offset_ = constants_.measured_index_position -
- constants_.known_index_pulse * constants_.index_difference -
- min_index_position_;
- zeroed_ = true;
- } else if (zeroed_ && !error_) {
- // Detect whether the index pulse is somewhere other than where we expect
- // it to be. First we compute the position of the most recent index pulse.
- double index_pulse_distance =
- info.latched_encoder() + offset_ - constants_.measured_index_position;
- // Second we compute the position of the index pulse in terms of
- // the index difference. I.e. if this index pulse is two pulses away from
- // the index pulse that we know about then this number should be positive
- // or negative two.
- double relative_distance =
- index_pulse_distance / constants_.index_difference;
- // Now we compute how far away the measured index pulse is from the
- // expected index pulse.
- double error = relative_distance - ::std::round(relative_distance);
- // This lets us check if the index pulse is within an acceptable error
- // margin of where we expected it to be.
- if (::std::abs(error) > constants_.allowable_encoder_error) {
- VLOG(1)
- << "Encoder ticks out of range since last index pulse. known index "
- "pulse: "
- << constants_.measured_index_position << ", expected index pulse: "
- << round(relative_distance) * constants_.index_difference +
- constants_.measured_index_position
- << ", actual index pulse: " << info.latched_encoder() + offset_
- << ", "
- "allowable error: "
- << constants_.allowable_encoder_error * constants_.index_difference;
- error_ = true;
- }
- }
-
- position_ = info.encoder() + offset_;
-}
-
-flatbuffers::Offset<PulseIndexZeroingEstimator::State>
-PulseIndexZeroingEstimator::GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const {
- State::Builder builder(*fbb);
- builder.add_error(error_);
- builder.add_zeroed(zeroed_);
- builder.add_position(position_);
- builder.add_min_index_position(min_index_position_);
- builder.add_max_index_position(max_index_position_);
- builder.add_index_pulses_seen(IndexPulseCount());
- return builder.Finish();
-}
-
-AbsoluteEncoderZeroingEstimator::AbsoluteEncoderZeroingEstimator(
- const constants::AbsoluteEncoderZeroingConstants &constants)
- : constants_(constants), move_detector_(constants_.moving_buffer_size) {
- relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
- Reset();
-}
-
-void AbsoluteEncoderZeroingEstimator::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 AbsoluteEncoderZeroingEstimator::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.";
- 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();
- 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();
-
- // Now, compute the estimated absolute position using the previously
- // estimated offset and the incremental encoder.
- const double adjusted_incremental_encoder =
- sample.encoder + average_relative_to_absolute_offset;
-
- // Now, compute the absolute encoder value nearest 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;
- }
-
- // Drop the oldest sample when we run this function the next time around.
- samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
-
- // And our offset is the offset that gives us the position within +- ord/2
- // of the middle position.
- offset_ = Wrap(constants_.middle_position,
- 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))));
-
- 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.
- position_ = offset_ + info.encoder();
-}
-
-flatbuffers::Offset<AbsoluteEncoderZeroingEstimator::State>
-AbsoluteEncoderZeroingEstimator::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_);
- return builder.Finish();
-}
-
-} // namespace zeroing
-} // namespace frc971
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index fe1b461..4d0c1cc 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -58,169 +58,6 @@
flatbuffers::FlatBufferBuilder *fbb) const = 0;
};
-// Estimates the position with an incremental encoder with an index pulse and a
-// potentiometer.
-class PotAndIndexPulseZeroingEstimator
- : public ZeroingEstimator<PotAndIndexPosition,
- constants::PotAndIndexPulseZeroingConstants,
- EstimatorState> {
- public:
- explicit PotAndIndexPulseZeroingEstimator(
- const constants::PotAndIndexPulseZeroingConstants &constants);
-
- // Update the internal logic with the next sensor values.
- void UpdateEstimate(const PotAndIndexPosition &info) override;
-
- // Reset the internal logic so it needs to be re-zeroed.
- void Reset() override;
-
- // Manually trigger an internal error. This is used for testing the error
- // logic.
- void TriggerError() override;
-
- bool error() const override { return error_; }
-
- bool zeroed() const override { return zeroed_; }
-
- double offset() const override { return offset_; }
-
- // Returns a number between 0 and 1 that represents the percentage of the
- // samples being used in the moving average filter. A value of 0.0 means that
- // no samples are being used. A value of 1.0 means that the filter is using
- // as many samples as it has room for. For example, after a Reset() this
- // value returns 0.0. As more samples get added with UpdateEstimate(...) the
- // return value starts increasing to 1.0.
- double offset_ratio_ready() const {
- return start_pos_samples_.size() /
- static_cast<double>(constants_.average_filter_size);
- }
-
- // Returns true if the sample buffer is full.
- bool offset_ready() const override {
- return start_pos_samples_.size() == constants_.average_filter_size;
- }
-
- // Returns information about our current state.
- virtual flatbuffers::Offset<State> GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const override;
-
- private:
- // This function calculates the start position given the internal state and
- // the provided `latched_encoder' value.
- double CalculateStartPosition(double start_average,
- double latched_encoder) const;
-
- // The zeroing constants used to describe the configuration of the system.
- const constants::PotAndIndexPulseZeroingConstants constants_;
-
- // The estimated position.
- double position_;
- // The unzeroed filtered position.
- double filtered_position_ = 0.0;
- // The next position in 'start_pos_samples_' to be used to store the next
- // sample.
- int samples_idx_;
- // Last 'max_sample_count_' samples for start positions.
- std::vector<double> start_pos_samples_;
- // The estimated starting position of the mechanism. We also call this the
- // 'offset' in some contexts.
- double offset_;
- // Flag for triggering logic that takes note of the current index pulse count
- // after a reset. See `last_used_index_pulse_count_'.
- bool wait_for_index_pulse_;
- // After a reset we keep track of the index pulse count with this. Only after
- // the index pulse count changes (i.e. increments at least once or wraps
- // around) will we consider the mechanism zeroed. We also use this to store
- // the most recent `PotAndIndexPosition::index_pulses' value when the start
- // position was calculated. It helps us calculate the start position only on
- // index pulses to reject corrupted intermediate data.
- uint32_t last_used_index_pulse_count_;
- // Marker to track whether we're fully zeroed yet or not.
- bool zeroed_;
- // Marker to track whether an error has occurred. This gets reset to false
- // whenever Reset() is called.
- bool error_;
- // Stores the position "start_pos" variable the first time the program
- // is zeroed.
- double first_start_pos_;
-};
-
-// Estimates the position with an incremental encoder and a hall effect sensor.
-class HallEffectAndPositionZeroingEstimator
- : public ZeroingEstimator<HallEffectAndPosition,
- constants::HallEffectZeroingConstants,
- HallEffectAndPositionEstimatorState> {
- public:
- explicit HallEffectAndPositionZeroingEstimator(
- const ZeroingConstants &constants);
-
- // Update the internal logic with the next sensor values.
- void UpdateEstimate(const Position &info) override;
-
- // Reset the internal logic so it needs to be re-zeroed.
- void Reset() override;
-
- // Manually trigger an internal error. This is used for testing the error
- // logic.
- void TriggerError() override;
-
- bool error() const override { return error_; }
-
- bool zeroed() const override { return zeroed_; }
-
- double offset() const override { return offset_; }
-
- bool offset_ready() const override { return zeroed_; }
-
- // Returns information about our current state.
- virtual flatbuffers::Offset<State> GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const override;
-
- private:
- // Sets the minimum and maximum posedge position values.
- void StoreEncoderMaxAndMin(const HallEffectAndPosition &info);
-
- // The zeroing constants used to describe the configuration of the system.
- const ZeroingConstants constants_;
-
- // The estimated state of the hall effect.
- double current_ = 0.0;
- // The estimated position.
- double position_ = 0.0;
- // The smallest and largest positions of the last set of encoder positions
- // while the hall effect was low.
- double min_low_position_;
- double max_low_position_;
- // If we've seen the hall effect high for enough times without going low, then
- // we can be sure it isn't a false positive.
- bool high_long_enough_;
- size_t cycles_high_;
-
- bool last_hall_ = false;
-
- // The estimated starting position of the mechanism. We also call this the
- // 'offset' in some contexts.
- double offset_;
- // Flag for triggering logic that takes note of the current posedge count
- // after a reset. See `last_used_posedge_count_'.
- bool initialized_;
- // After a reset we keep track of the posedge count with this. Only after the
- // posedge count changes (i.e. increments at least once or wraps around) will
- // we consider the mechanism zeroed. We also use this to store the most recent
- // `HallEffectAndPosition::posedge_count' value when the start position
- // was calculated. It helps us calculate the start position only on posedges
- // to reject corrupted intermediate data.
- int32_t last_used_posedge_count_;
- // Marker to track whether we're fully zeroed yet or not.
- bool zeroed_;
- // Marker to track whether an error has occurred. This gets reset to false
- // whenever Reset() is called.
- bool error_ = false;
- // Stores the position "start_pos" variable the first time the program
- // is zeroed.
- double first_start_pos_;
-};
-
// Class to encapsulate the logic to decide when we are moving and which samples
// are safe to use.
template <typename Position, typename PositionBuffer>
@@ -287,237 +124,7 @@
size_t buffered_samples_idx_;
};
-// Estimates the position with an absolute encoder which also reports
-// incremental counts, and a potentiometer.
-class PotAndAbsoluteEncoderZeroingEstimator
- : public ZeroingEstimator<PotAndAbsolutePosition,
- constants::PotAndAbsoluteEncoderZeroingConstants,
- PotAndAbsoluteEncoderEstimatorState> {
- public:
- explicit PotAndAbsoluteEncoderZeroingEstimator(
- const constants::PotAndAbsoluteEncoderZeroingConstants &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 PotAndAbsolutePosition &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 PotAndAbsolutePosition &position_buffer)
- : absolute_encoder(position_buffer.absolute_encoder()),
- encoder(position_buffer.encoder()),
- pot(position_buffer.pot()) {}
- double absolute_encoder;
- double encoder;
- double pot;
- };
-
- // The zeroing constants used to describe the configuration of the system.
- const constants::PotAndAbsoluteEncoderZeroingConstants 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_;
- // Offset between the Pot and Relative encoder position.
- ::std::vector<double> offset_samples_;
-
- MoveDetector<PositionStruct, PotAndAbsolutePosition> move_detector_;
-
- // Estimated offset between the pot and relative encoder.
- double pot_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;
-};
-
-// Zeros by seeing all the index pulses in the range of motion of the mechanism
-// and using that to figure out which index pulse is which.
-class PulseIndexZeroingEstimator
- : public ZeroingEstimator<IndexPosition,
- constants::EncoderPlusIndexZeroingConstants,
- IndexEstimatorState> {
- public:
- explicit PulseIndexZeroingEstimator(const ZeroingConstants &constants)
- : constants_(constants) {
- Reset();
- }
-
- // Resets the internal logic so it needs to be re-zeroed.
- void Reset() override;
-
- bool zeroed() const override { return zeroed_; }
-
- // It's as ready as it'll ever be...
- bool offset_ready() const override { return true; }
-
- double offset() const override { return offset_; }
-
- bool error() const override { return error_; }
-
- // Updates the internal logic with the next sensor values.
- void UpdateEstimate(const IndexPosition &info) override;
-
- // Returns information about our current state.
- virtual flatbuffers::Offset<State> GetEstimatorState(
- flatbuffers::FlatBufferBuilder *fbb) const override;
-
- void TriggerError() override { error_ = true; }
-
- private:
- // Returns the current real position using the relative encoder offset.
- double CalculateCurrentPosition(const IndexPosition &info);
-
- // Sets the minimum and maximum index pulse position values.
- void StoreIndexPulseMaxAndMin(const IndexPosition &info);
-
- // Returns the number of index pulses we should have seen so far.
- int IndexPulseCount() const;
-
- // Contains the physical constants describing the system.
- const ZeroingConstants constants_;
-
- // The smallest position of all the index pulses.
- double min_index_position_;
- // The largest position of all the index pulses.
- double max_index_position_;
-
- // The estimated starting position of the mechanism.
- double offset_;
- // After a reset we keep track of the index pulse count with this. Only after
- // the index pulse count changes (i.e. increments at least once or wraps
- // around) will we consider the mechanism zeroed. We also use this to store
- // the most recent `PotAndIndexPosition::index_pulses' value when the start
- // position was calculated. It helps us calculate the start position only on
- // index pulses to reject corrupted intermediate data.
- uint32_t last_used_index_pulse_count_;
-
- // True if we are fully zeroed.
- bool zeroed_;
- // Marker to track whether an error has occurred.
- bool error_;
-
- // The estimated position.
- double position_;
-};
-
-// Estimates the position with an absolute encoder which also reports
-// incremental counts. The absolute encoder can't spin more than one
-// revolution.
-class AbsoluteEncoderZeroingEstimator
- : public ZeroingEstimator<AbsolutePosition,
- constants::AbsoluteEncoderZeroingConstants,
- AbsoluteEncoderEstimatorState> {
- public:
- explicit AbsoluteEncoderZeroingEstimator(
- const constants::AbsoluteEncoderZeroingConstants &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::AbsoluteEncoderZeroingConstants 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;
-};
-
+// A trivial ZeroingEstimator which just passes the position straight through.
class RelativeEncoderZeroingEstimator
: public ZeroingEstimator<RelativePosition, void,
RelativeEncoderEstimatorState> {
@@ -562,4 +169,12 @@
} // namespace zeroing
} // namespace frc971
+// TODO(Brian): Actually split these targets apart. Need to convert all the
+// reverse dependencies to #include what they actually need...
+#include "frc971/zeroing/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"
+#include "frc971/zeroing/pulse_index.h"
+
#endif // FRC971_ZEROING_ZEROING_H_
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
deleted file mode 100644
index 2b715ea..0000000
--- a/frc971/zeroing/zeroing_test.cc
+++ /dev/null
@@ -1,817 +0,0 @@
-#include "frc971/zeroing/zeroing.h"
-
-#include <unistd.h>
-
-#include <memory>
-#include <random>
-
-#include "aos/die.h"
-#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/control_loops/position_sensor_sim.h"
-#include "gtest/gtest.h"
-
-namespace frc971 {
-namespace zeroing {
-
-using constants::AbsoluteEncoderZeroingConstants;
-using constants::EncoderPlusIndexZeroingConstants;
-using constants::PotAndAbsoluteEncoderZeroingConstants;
-using constants::PotAndIndexPulseZeroingConstants;
-using control_loops::PositionSensorSimulator;
-using FBB = flatbuffers::FlatBufferBuilder;
-
-static const size_t kSampleSize = 30;
-static const double kAcceptableUnzeroedError = 0.2;
-static const double kIndexErrorFraction = 0.3;
-static const size_t kMovingBufferSize = 3;
-
-class ZeroingTest : public ::testing::Test {
- protected:
- void SetUp() override {}
-
- void MoveTo(PositionSensorSimulator *simulator,
- PotAndIndexPulseZeroingEstimator *estimator,
- double new_position) {
- simulator->MoveTo(new_position);
- FBB fbb;
- estimator->UpdateEstimate(
- *simulator->FillSensorValues<PotAndIndexPosition>(&fbb));
- }
-
- void MoveTo(PositionSensorSimulator *simulator,
- AbsoluteEncoderZeroingEstimator *estimator, double new_position) {
- simulator->MoveTo(new_position);
- FBB fbb;
- estimator->UpdateEstimate(
- *simulator->FillSensorValues<AbsolutePosition>(&fbb));
- }
-
- void MoveTo(PositionSensorSimulator *simulator,
- PotAndAbsoluteEncoderZeroingEstimator *estimator,
- double new_position) {
- simulator->MoveTo(new_position);
- FBB fbb;
- estimator->UpdateEstimate(
- *simulator->FillSensorValues<PotAndAbsolutePosition>(&fbb));
- }
-
- void MoveTo(PositionSensorSimulator *simulator,
- PulseIndexZeroingEstimator *estimator, double new_position) {
- simulator->MoveTo(new_position);
- FBB fbb;
- estimator->UpdateEstimate(
- *simulator->FillSensorValues<IndexPosition>(&fbb));
- }
-
- void MoveTo(PositionSensorSimulator *simulator,
- HallEffectAndPositionZeroingEstimator *estimator,
- double new_position) {
- simulator->MoveTo(new_position);
- FBB fbb;
- estimator->UpdateEstimate(
- *simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
- }
-
- void MoveTo(PositionSensorSimulator *simulator,
- RelativeEncoderZeroingEstimator *estimator, double new_position) {
- simulator->MoveTo(new_position);
- FBB fbb;
- estimator->UpdateEstimate(
- *simulator->FillSensorValues<RelativePosition>(&fbb));
- }
-
- template <typename T>
- double GetEstimatorPosition(T *estimator) {
- FBB fbb;
- fbb.Finish(estimator->GetEstimatorState(&fbb));
- return flatbuffers::GetRoot<typename T::State>(fbb.GetBufferPointer())
- ->position();
- }
-};
-
-TEST_F(ZeroingTest, TestMovingAverageFilter) {
- const double index_diff = 1.0;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(3.6 * index_diff, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- // The zeroing code is supposed to perform some filtering on the difference
- // between the potentiometer value and the encoder value. We assume that 300
- // samples are sufficient to have updated the filter.
- for (int i = 0; i < 300; i++) {
- MoveTo(&sim, &estimator, 3.3 * index_diff);
- }
- ASSERT_NEAR(3.3 * index_diff, GetEstimatorPosition(&estimator),
- kAcceptableUnzeroedError * index_diff);
-
- for (int i = 0; i < 300; i++) {
- MoveTo(&sim, &estimator, 3.9 * index_diff);
- }
- ASSERT_NEAR(3.9 * index_diff, GetEstimatorPosition(&estimator),
- kAcceptableUnzeroedError * index_diff);
-}
-
-TEST_F(ZeroingTest, NotZeroedBeforeEnoughSamplesCollected) {
- double index_diff = 0.5;
- double position = 3.6 * index_diff;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(position, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- // Make sure that the zeroing code does not consider itself zeroed until we
- // collect a good amount of samples. In this case we're waiting until the
- // moving average filter is full.
- for (unsigned int i = 0; i < kSampleSize - 1; i++) {
- MoveTo(&sim, &estimator, position += index_diff);
- ASSERT_FALSE(estimator.zeroed());
- }
-
- MoveTo(&sim, &estimator, position);
- ASSERT_TRUE(estimator.zeroed());
-}
-
-TEST_F(ZeroingTest, TestLotsOfMovement) {
- double index_diff = 1.0;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(3.6, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- // The zeroing code is supposed to perform some filtering on the difference
- // between the potentiometer value and the encoder value. We assume that 300
- // samples are sufficient to have updated the filter.
- for (int i = 0; i < 300; i++) {
- MoveTo(&sim, &estimator, 3.6);
- }
- ASSERT_NEAR(3.6, GetEstimatorPosition(&estimator),
- kAcceptableUnzeroedError * index_diff);
-
- // With a single index pulse the zeroing estimator should be able to lock
- // onto the true value of the position.
- MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
-}
-
-TEST_F(ZeroingTest, TestDifferentIndexDiffs) {
- double index_diff = 0.89;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- // The zeroing code is supposed to perform some filtering on the difference
- // between the potentiometer value and the encoder value. We assume that 300
- // samples are sufficient to have updated the filter.
- for (int i = 0; i < 300; i++) {
- MoveTo(&sim, &estimator, 3.5 * index_diff);
- }
- ASSERT_NEAR(3.5 * index_diff, GetEstimatorPosition(&estimator),
- kAcceptableUnzeroedError * index_diff);
-
- // With a single index pulse the zeroing estimator should be able to lock
- // onto the true value of the position.
- MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
-
- MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
-}
-
-TEST_F(ZeroingTest, TestPercentage) {
- double index_diff = 0.89;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(3.5 * index_diff, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- for (unsigned int i = 0; i < kSampleSize / 2; i++) {
- MoveTo(&sim, &estimator, 3.5 * index_diff);
- }
- ASSERT_NEAR(0.5, estimator.offset_ratio_ready(), 0.001);
- ASSERT_FALSE(estimator.offset_ready());
-
- for (unsigned int i = 0; i < kSampleSize / 2; i++) {
- MoveTo(&sim, &estimator, 3.5 * index_diff);
- }
- ASSERT_NEAR(1.0, estimator.offset_ratio_ready(), 0.001);
- ASSERT_TRUE(estimator.offset_ready());
-}
-
-TEST_F(ZeroingTest, TestOffset) {
- double index_diff = 0.89;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- MoveTo(&sim, &estimator, 3.1 * index_diff);
-
- for (unsigned int i = 0; i < kSampleSize; i++) {
- MoveTo(&sim, &estimator, 5.0 * index_diff);
- }
-
- ASSERT_NEAR(3.1 * index_diff, estimator.offset(), 0.001);
-}
-
-TEST_F(ZeroingTest, WaitForIndexPulseAfterReset) {
- double index_diff = 0.6;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(3.1 * index_diff, index_diff / 3.0);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
-
- // Make sure to fill up the averaging filter with samples.
- for (unsigned int i = 0; i < kSampleSize; i++) {
- MoveTo(&sim, &estimator, 3.1 * 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, 4.5 * index_diff);
- ASSERT_TRUE(estimator.zeroed());
-
- // Reset the zeroing logic and supply a bunch of samples within the current
- // index segment.
- estimator.Reset();
- for (unsigned int i = 0; i < kSampleSize; i++) {
- MoveTo(&sim, &estimator, 4.2 * index_diff);
- }
-
- // Make sure we're not zeroed until we hit an index pulse.
- ASSERT_FALSE(estimator.zeroed());
-
- // Trigger another index pulse; we should be zeroed again.
- MoveTo(&sim, &estimator, 3.1 * index_diff);
- 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);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, known_index_pos, kIndexErrorFraction});
-
- // 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, GetEstimatorPosition(&estimator));
-
- // 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, GetEstimatorPosition(&estimator));
-}
-
-TEST_F(ZeroingTest, BasicErrorAPITest) {
- const double index_diff = 1.0;
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- kSampleSize, index_diff, 0.0, kIndexErrorFraction});
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
-
- // Perform a simple move and make sure that no error occured.
- MoveTo(&sim, &estimator, 3.5 * index_diff);
- ASSERT_FALSE(estimator.error());
-
- // Trigger an error and make sure it's reported.
- estimator.TriggerError();
- ASSERT_TRUE(estimator.error());
-
- // Make sure that it can recover after a reset.
- estimator.Reset();
- ASSERT_FALSE(estimator.error());
- MoveTo(&sim, &estimator, 4.5 * index_diff);
- MoveTo(&sim, &estimator, 5.5 * index_diff);
- ASSERT_FALSE(estimator.error());
-}
-
-// Tests that an error is detected when the starting position changes too much.
-TEST_F(ZeroingTest, TestIndexOffsetError) {
- const double index_diff = 0.8;
- const double known_index_pos = 2 * index_diff;
- const size_t sample_size = 30;
- PositionSensorSimulator sim(index_diff);
- sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
- PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
- sample_size, index_diff, known_index_pos, kIndexErrorFraction});
-
- for (size_t i = 0; i < sample_size; i++) {
- MoveTo(&sim, &estimator, 13 * index_diff);
- }
- MoveTo(&sim, &estimator, 8 * index_diff);
-
- ASSERT_TRUE(estimator.zeroed());
- ASSERT_FALSE(estimator.error());
- sim.Initialize(9.0 * index_diff + 0.31 * index_diff, index_diff / 3.0,
- known_index_pos);
- MoveTo(&sim, &estimator, 9 * index_diff);
- ASSERT_TRUE(estimator.zeroed());
- ASSERT_TRUE(estimator.error());
-}
-
-// Makes sure that using an absolute encoder lets us zero without moving.
-TEST_F(ZeroingTest, TestPotAndAbsoluteEncoderZeroingWithoutMovement) {
- const double index_diff = 1.0;
- PositionSensorSimulator sim(index_diff);
-
- const double start_pos = 2.1;
- double measured_absolute_position = 0.3 * index_diff;
-
- PotAndAbsoluteEncoderZeroingConstants 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);
-
- PotAndAbsoluteEncoderZeroingEstimator 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(ZeroingTest, TestPotAndAbsoluteEncoderZeroingIgnoresNAN) {
- const double index_diff = 1.0;
- PositionSensorSimulator sim(index_diff);
-
- const double start_pos = 2.1;
- double measured_absolute_position = 0.3 * index_diff;
-
- PotAndAbsoluteEncoderZeroingConstants 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);
-
- PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
-
- // We tolerate a couple NANs before we start.
- FBB fbb;
- fbb.Finish(CreatePotAndAbsolutePosition(
- fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
- for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(
- *flatbuffers::GetRoot<PotAndAbsolutePosition>(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(ZeroingTest, TestPotAndAbsoluteEncoderZeroingWithMovement) {
- 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;
-
- PotAndAbsoluteEncoderZeroingConstants 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);
-
- PotAndAbsoluteEncoderZeroingEstimator 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(ZeroingTest, TestPotAndAbsoluteEncoderZeroingWithNaN) {
- PotAndAbsoluteEncoderZeroingConstants constants{
- kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
-
- PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
-
- FBB fbb;
- fbb.Finish(CreatePotAndAbsolutePosition(
- fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
- const auto sensor_values =
- flatbuffers::GetRoot<PotAndAbsolutePosition>(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());
-}
-
-// Tests that an error is detected when the starting position changes too much.
-TEST_F(ZeroingTest, TestRelativeEncoderZeroing) {
- EncoderPlusIndexZeroingConstants constants;
- constants.index_pulse_count = 3;
- constants.index_difference = 10.0;
- constants.measured_index_position = 20.0;
- constants.known_index_pulse = 1;
- constants.allowable_encoder_error = 0.01;
-
- PositionSensorSimulator sim(constants.index_difference);
-
- const double start_pos = 2.5 * constants.index_difference;
-
- sim.Initialize(start_pos, constants.index_difference / 3.0,
- constants.measured_index_position);
-
- PulseIndexZeroingEstimator estimator(constants);
-
- // Should not be zeroed when we stand still.
- for (int i = 0; i < 300; ++i) {
- MoveTo(&sim, &estimator, start_pos);
- ASSERT_FALSE(estimator.zeroed());
- }
-
- // Move to 1.5 constants.index_difference and we should still not be zeroed.
- MoveTo(&sim, &estimator, 1.5 * constants.index_difference);
- ASSERT_FALSE(estimator.zeroed());
-
- // Move to 0.5 constants.index_difference and we should still not be zeroed.
- MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
- ASSERT_FALSE(estimator.zeroed());
-
- // Move back to 1.5 constants.index_difference and we should still not be
- // zeroed.
- MoveTo(&sim, &estimator, 1.5 * constants.index_difference);
- ASSERT_FALSE(estimator.zeroed());
-
- // Move back to 2.5 constants.index_difference and we should still not be
- // zeroed.
- MoveTo(&sim, &estimator, 2.5 * constants.index_difference);
- ASSERT_FALSE(estimator.zeroed());
-
- // Move back to 3.5 constants.index_difference and we should now be zeroed.
- MoveTo(&sim, &estimator, 3.5 * constants.index_difference);
- ASSERT_TRUE(estimator.zeroed());
-
- ASSERT_DOUBLE_EQ(start_pos, estimator.offset());
- ASSERT_DOUBLE_EQ(3.5 * constants.index_difference,
- GetEstimatorPosition(&estimator));
-
- MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
- ASSERT_DOUBLE_EQ(0.5 * constants.index_difference,
- GetEstimatorPosition(&estimator));
-}
-
-// Tests that we can detect when an index pulse occurs where we didn't expect
-// it to for the PulseIndexZeroingEstimator.
-TEST_F(ZeroingTest, TestRelativeEncoderSlipping) {
- EncoderPlusIndexZeroingConstants constants;
- constants.index_pulse_count = 3;
- constants.index_difference = 10.0;
- constants.measured_index_position = 20.0;
- constants.known_index_pulse = 1;
- constants.allowable_encoder_error = 0.05;
-
- PositionSensorSimulator sim(constants.index_difference);
-
- const double start_pos =
- constants.measured_index_position + 0.5 * constants.index_difference;
-
- for (double direction : {1.0, -1.0}) {
- sim.Initialize(start_pos, constants.index_difference / 3.0,
- constants.measured_index_position);
-
- PulseIndexZeroingEstimator estimator(constants);
-
- // Zero the estimator.
- MoveTo(&sim, &estimator, start_pos - 1 * constants.index_difference);
- MoveTo(
- &sim, &estimator,
- start_pos - constants.index_pulse_count * constants.index_difference);
- ASSERT_TRUE(estimator.zeroed());
- ASSERT_FALSE(estimator.error());
-
- // We have a 5% allowable error so we slip a little bit each time and make
- // sure that the index pulses are still accepted.
- for (double error = 0.00;
- ::std::abs(error) < constants.allowable_encoder_error;
- error += 0.01 * direction) {
- sim.Initialize(start_pos, constants.index_difference / 3.0,
- constants.measured_index_position +
- error * constants.index_difference);
- MoveTo(&sim, &estimator, start_pos - constants.index_difference);
- EXPECT_FALSE(estimator.error());
- }
-
- // As soon as we hit cross the error margin, we should trigger an error.
- sim.Initialize(start_pos, constants.index_difference / 3.0,
- constants.measured_index_position +
- constants.allowable_encoder_error * 1.1 *
- constants.index_difference * direction);
- MoveTo(&sim, &estimator, start_pos - constants.index_difference);
- ASSERT_TRUE(estimator.error());
- }
-}
-
-// Test fixture for HallEffectAndPositionZeroingEstimator.
-class HallEffectAndPositionZeroingEstimatorTest : public ZeroingTest {
- public:
- // The starting position of the system.
- static constexpr double kStartPosition = 2.0;
-
- // Returns a reasonable set of test constants.
- static constants::HallEffectZeroingConstants MakeConstants() {
- constants::HallEffectZeroingConstants constants;
- constants.lower_hall_position = 0.25;
- constants.upper_hall_position = 0.75;
- constants.index_difference = 1.0;
- constants.hall_trigger_zeroing_length = 2;
- constants.zeroing_move_direction = false;
- return constants;
- }
-
- HallEffectAndPositionZeroingEstimatorTest()
- : constants_(MakeConstants()), sim_(constants_.index_difference) {
- // Start the system out at the starting position.
- sim_.InitializeHallEffectAndPosition(kStartPosition,
- constants_.lower_hall_position,
- constants_.upper_hall_position);
- }
-
- protected:
- // Constants, and the simulation using them.
- const constants::HallEffectZeroingConstants constants_;
- PositionSensorSimulator sim_;
-};
-
-// Tests that an error is detected when the starting position changes too much.
-TEST_F(HallEffectAndPositionZeroingEstimatorTest, TestHallEffectZeroing) {
- HallEffectAndPositionZeroingEstimator estimator(constants_);
-
- // Should not be zeroed when we stand still.
- for (int i = 0; i < 300; ++i) {
- MoveTo(&sim_, &estimator, kStartPosition);
- ASSERT_FALSE(estimator.zeroed());
- }
-
- MoveTo(&sim_, &estimator, 1.9);
- ASSERT_FALSE(estimator.zeroed());
-
- // Move to where the hall effect is triggered and make sure it becomes zeroed.
- MoveTo(&sim_, &estimator, 1.5);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 1.5);
- ASSERT_TRUE(estimator.zeroed());
-
- // Check that the offset is calculated correctly. We should expect to read
- // 0.5. Since the encoder is reading -0.5 right now, the offset needs to be
- // 1.
- EXPECT_DOUBLE_EQ(1.0, estimator.offset());
-
- // Make sure triggering errors works.
- estimator.TriggerError();
- ASSERT_TRUE(estimator.error());
-
- // Ensure resetting resets the state of the estimator.
- estimator.Reset();
- ASSERT_FALSE(estimator.zeroed());
- ASSERT_FALSE(estimator.error());
-}
-
-// Tests that we don't zero on a too short pulse.
-TEST_F(HallEffectAndPositionZeroingEstimatorTest, TestTooShortPulse) {
- HallEffectAndPositionZeroingEstimator estimator(constants_);
-
- // Trigger for 1 cycle.
- MoveTo(&sim_, &estimator, 0.9);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.5);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.9);
- EXPECT_FALSE(estimator.zeroed());
-}
-
-// Tests that we don't zero when we go the wrong direction.
-TEST_F(HallEffectAndPositionZeroingEstimatorTest, TestWrongDirectionNoZero) {
- HallEffectAndPositionZeroingEstimator estimator(constants_);
-
- // Pass through the sensor, lingering long enough that we should zero.
- MoveTo(&sim_, &estimator, 0.0);
- ASSERT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.4);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.6);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.7);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.9);
- EXPECT_FALSE(estimator.zeroed());
-}
-
-// Make sure we don't zero if we start in the hall effect's range.
-TEST_F(HallEffectAndPositionZeroingEstimatorTest, TestStartingOnNoZero) {
- HallEffectAndPositionZeroingEstimator estimator(constants_);
- MoveTo(&sim_, &estimator, 0.5);
- estimator.Reset();
-
- // Stay on the hall effect. We shouldn't zero.
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.5);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.5);
- EXPECT_FALSE(estimator.zeroed());
-
- // Verify moving off the hall still doesn't zero us.
- MoveTo(&sim_, &estimator, 0.0);
- EXPECT_FALSE(estimator.zeroed());
- MoveTo(&sim_, &estimator, 0.0);
- EXPECT_FALSE(estimator.zeroed());
-}
-
-// Makes sure that using an absolute encoder lets us zero without moving.
-TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithoutMovement) {
- const double index_diff = 1.0;
- PositionSensorSimulator sim(index_diff);
-
- const double kMiddlePosition = 2.5;
- const double start_pos = 2.1;
- double measured_absolute_position = 0.3 * index_diff;
-
- AbsoluteEncoderZeroingConstants constants{
- kSampleSize, index_diff, measured_absolute_position,
- kMiddlePosition, 0.1, kMovingBufferSize,
- kIndexErrorFraction};
-
- sim.Initialize(start_pos, index_diff / 3.0, 0.0,
- constants.measured_absolute_position);
-
- AbsoluteEncoderZeroingEstimator 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(ZeroingTest, TestAbsoluteEncoderZeroingIgnoresNAN) {
- const double index_diff = 1.0;
- PositionSensorSimulator sim(index_diff);
-
- const double start_pos = 2.1;
- double measured_absolute_position = 0.3 * index_diff;
- const double kMiddlePosition = 2.5;
-
- AbsoluteEncoderZeroingConstants constants{
- kSampleSize, index_diff, measured_absolute_position,
- kMiddlePosition, 0.1, kMovingBufferSize,
- kIndexErrorFraction};
-
- sim.Initialize(start_pos, index_diff / 3.0, 0.0,
- constants.measured_absolute_position);
-
- AbsoluteEncoderZeroingEstimator estimator(constants);
-
- // We tolerate a couple NANs before we start.
- FBB 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());
- EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
-}
-
-// Makes sure that using an absolute encoder doesn't let us zero while moving.
-TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithMovement) {
- 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;
- const double kMiddlePosition = 2.5;
-
- AbsoluteEncoderZeroingConstants constants{
- kSampleSize, index_diff, measured_absolute_position,
- kMiddlePosition, 0.1, kMovingBufferSize,
- kIndexErrorFraction};
-
- sim.Initialize(start_pos, index_diff / 3.0, 0.0,
- constants.measured_absolute_position);
-
- AbsoluteEncoderZeroingEstimator 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(ZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
- AbsoluteEncoderZeroingConstants constants{
- kSampleSize, 1, 0.3, 1.0, 0.1, kMovingBufferSize, kIndexErrorFraction};
-
- AbsoluteEncoderZeroingEstimator estimator(constants);
-
- FBB 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());
-}
-
-TEST_F(ZeroingTest, TestRelativeEncoderZeroingWithoutMovement) {
- PositionSensorSimulator sim(1.0);
- RelativeEncoderZeroingEstimator estimator;
-
- sim.InitializeRelativeEncoder();
-
- ASSERT_TRUE(estimator.zeroed());
- ASSERT_TRUE(estimator.offset_ready());
- EXPECT_DOUBLE_EQ(estimator.offset(), 0.0);
- EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.0);
-
- MoveTo(&sim, &estimator, 0.1);
-
- EXPECT_DOUBLE_EQ(GetEstimatorPosition(&estimator), 0.1);
-}
-
-} // namespace zeroing
-} // namespace frc971
diff --git a/frc971/zeroing/zeroing_test.h b/frc971/zeroing/zeroing_test.h
new file mode 100644
index 0000000..9b7ba95
--- /dev/null
+++ b/frc971/zeroing/zeroing_test.h
@@ -0,0 +1,32 @@
+#include "frc971/zeroing/zeroing.h"
+
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using control_loops::PositionSensorSimulator;
+using FBB = flatbuffers::FlatBufferBuilder;
+
+constexpr size_t kSampleSize = 30;
+constexpr double kAcceptableUnzeroedError = 0.2;
+constexpr double kIndexErrorFraction = 0.3;
+constexpr size_t kMovingBufferSize = 3;
+
+class ZeroingTest : public ::testing::Test {
+ protected:
+ template <typename T>
+ double GetEstimatorPosition(T *estimator) {
+ FBB fbb;
+ fbb.Finish(estimator->GetEstimatorState(&fbb));
+ return flatbuffers::GetRoot<typename T::State>(fbb.GetBufferPointer())
+ ->position();
+ }
+};
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971