Refactor moving logic out from zeroing.cc
We want to reuse it for the absolute encoder zeroing.
Change-Id: I33770f8ecb76f5b5f7cae375ac7fdb4f73432fa6
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 7bf2b27..114ef21 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -248,10 +248,9 @@
PotAndAbsoluteEncoderZeroingEstimator::PotAndAbsoluteEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
- : constants_(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);
- buffered_samples_.reserve(constants_.moving_buffer_size);
Reset();
}
@@ -266,7 +265,7 @@
nan_samples_ = 0;
relative_to_absolute_offset_samples_.clear();
offset_samples_.clear();
- buffered_samples_.clear();
+ move_detector_.Reset();
error_ = false;
}
@@ -311,37 +310,11 @@
return;
}
- bool moving = true;
- if (buffered_samples_.size() < constants_.moving_buffer_size) {
- // Not enough samples to start determining if the robot is moving or not,
- // don't use the samples yet.
- buffered_samples_.push_back(info);
- } else {
- // Have enough samples to start determining if the robot is moving or not.
- buffered_samples_[buffered_samples_idx_] = info;
- const auto minmax_value = ::std::minmax_element(
- buffered_samples_.begin(), buffered_samples_.end(),
- [](const PotAndAbsolutePosition &left,
- const PotAndAbsolutePosition &right) {
- return left.encoder < right.encoder;
- });
- const double min_value = minmax_value.first->encoder;
- const double max_value = minmax_value.second->encoder;
-
- if (::std::abs(max_value - min_value) < constants_.zeroing_threshold) {
- // Robot isn't moving, use middle sample to determine offsets.
- moving = false;
- }
- }
- buffered_samples_idx_ =
- (buffered_samples_idx_ + 1) % constants_.moving_buffer_size;
+ const bool moving = move_detector_.Update(info, constants_.moving_buffer_size,
+ constants_.zeroing_threshold);
if (!moving) {
- // The robot is not moving, use the middle sample to determine offsets.
- const int middle_index =
- (buffered_samples_idx_ + (constants_.moving_buffer_size - 1) / 2) %
- constants_.moving_buffer_size;
- const PotAndAbsolutePosition &sample = buffered_samples_[middle_index];
+ const PotAndAbsolutePosition &sample = move_detector_.GetSample();
// Compute the average offset between the absolute encoder and relative
// encoder. If we have 0 samples, assume it is 0.
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 4d4f13d..e969a88 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -1,6 +1,8 @@
#ifndef FRC971_ZEROING_ZEROING_H_
#define FRC971_ZEROING_ZEROING_H_
+#include <algorithm>
+#include <cmath>
#include <cstdint>
#include <vector>
@@ -213,6 +215,71 @@
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>
+class MoveDetector {
+ public:
+ MoveDetector(size_t filter_size) {
+ buffered_samples_.reserve(filter_size);
+ Reset();
+ }
+
+ // Clears all the state.
+ void Reset() {
+ buffered_samples_.clear();
+ buffered_samples_idx_ = 0;
+ }
+
+ // Updates the detector with a new sample. Returns true if we are moving.
+ // buffer_size is the number of samples in the moving buffer, and
+ // zeroing_threshold is the max amount we can move within the period specified
+ // by buffer_size.
+ bool Update(const Position &position, size_t buffer_size,
+ double zeroing_threshold) {
+ bool moving = true;
+ if (buffered_samples_.size() < buffer_size) {
+ // Not enough samples to start determining if the robot is moving or not,
+ // don't use the samples yet.
+ buffered_samples_.push_back(position);
+ } else {
+ // Have enough samples to start determining if the robot is moving or not.
+ buffered_samples_[buffered_samples_idx_] = position;
+ const auto minmax_value = ::std::minmax_element(
+ buffered_samples_.begin(), buffered_samples_.end(),
+ [](const Position &left, const Position &right) {
+ return left.encoder < right.encoder;
+ });
+ const double min_value = minmax_value.first->encoder;
+ const double max_value = minmax_value.second->encoder;
+
+ if (::std::abs(max_value - min_value) < zeroing_threshold) {
+ // Robot isn't moving, use middle sample to determine offsets.
+ moving = false;
+ }
+ }
+ buffered_samples_idx_ = (buffered_samples_idx_ + 1) % buffer_size;
+ return moving;
+ }
+
+ // Returns a safe sample if we aren't moving as reported by Update.
+ const Position &GetSample() const {
+ // The robot is not moving, use the middle sample to determine offsets.
+ // The middle sample makes it so that we don't use the samples from the
+ // beginning or end of periods when the robot is moving.
+ const int middle_index =
+ (buffered_samples_idx_ + (buffered_samples_.size() / 2)) %
+ buffered_samples_.size();
+ return buffered_samples_[middle_index];
+ }
+
+ private:
+ // The last buffer_size samples.
+ ::std::vector<Position> buffered_samples_;
+ // The index to place the next sample in.
+ size_t buffered_samples_idx_;
+};
+
// Estimates the position with an absolute encoder which also reports
// incremental counts, and a potentiometer.
class PotAndAbsoluteEncoderZeroingEstimator
@@ -267,11 +334,9 @@
::std::vector<double> relative_to_absolute_offset_samples_;
// Offset between the Pot and Relative encoder position.
::std::vector<double> offset_samples_;
- // Last moving_buffer_size position samples to be used to determine if the
- // robot is moving.
- ::std::vector<PotAndAbsolutePosition> buffered_samples_;
- // Pointer to front of the buffered samples.
- int buffered_samples_idx_ = 0;
+
+ MoveDetector<PotAndAbsolutePosition> move_detector_;
+
// Estimated offset between the pot and relative encoder.
double pot_relative_encoder_offset_ = 0;
// Estimated start position of the mechanism