Logged the filtered but not zeroed position.
Change-Id: Ica47de1c6b84fd71540428cba06ac266d2e899fe
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 5184ad4..e8cac6f 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -11,6 +11,7 @@
state->error = estimator.error();
state->zeroed = estimator.zeroed();
state->position = estimator.position();
+ state->pot_position = estimator.filtered_position();
}
ZeroingEstimator::ZeroingEstimator(
@@ -122,6 +123,7 @@
}
pos_ = start_pos_ + info.encoder;
+ filtered_position_ = start_average + info.encoder;
}
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 433745a..5216df0 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -53,6 +53,10 @@
// some contexts we refer to this as the "offset".
double offset() const { return start_pos_; }
+ // Return the estimated position of the corresponding mechanism not using the
+ // index pulse, even if one is available.
+ double filtered_position() const { return filtered_position_; }
+
// 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
@@ -76,6 +80,8 @@
// The estimated position.
double pos_;
+ // The unzeroed filtered position.
+ double filtered_position_ = 0.0;
// The distance between two consecutive index positions.
double index_diff_;
// The next position in 'start_pos_samples_' to be used to store the next