Add zeroing estimator class.
Change-Id: I355db861a289a4f0f24eb2ca3e84a4e21270bfce
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
new file mode 100644
index 0000000..a0d3ba4
--- /dev/null
+++ b/frc971/zeroing/zeroing.cc
@@ -0,0 +1,50 @@
+#include "frc971/zeroing/zeroing.h"
+#include <math.h>
+#include <vector>
+
+namespace frc971 {
+namespace zeroing {
+
+ZeroingEstimator::ZeroingEstimator(double index_difference,
+ size_t max_sample_count) {
+ index_diff_ = index_difference;
+ samples_idx_ = 0;
+ max_sample_count_ = max_sample_count;
+ start_pos_samples_.reserve(max_sample_count);
+}
+
+void ZeroingEstimator::UpdateEstimate(const ZeroingInfo & info) {
+ if (start_pos_samples_.size() < max_sample_count_) {
+ start_pos_samples_.push_back(info.pot - info.encoder);
+ } else {
+ start_pos_samples_[samples_idx_] = info.pot - info.encoder;
+ }
+ samples_idx_ = (samples_idx_ + 1) % max_sample_count_;
+
+ double start_average = 0.0;
+ for (size_t i = 0; i < start_pos_samples_.size(); ++i) {
+ start_average += start_pos_samples_[i];
+ }
+
+ // Calculates the average of the starting position.
+ start_average = start_average / start_pos_samples_.size();
+ /* If the index_encoder is invalid, then we use
+ * the average of the starting position to
+ * calculate the position.
+ */
+ if (info.index_count == 0) {
+ pos_ = start_average + info.encoder;
+ } else {
+ // We calculate an aproximation of the value of the last index position.
+ double index_pos = start_average + info.index_encoder;
+ // We round index_pos to the closest valid value of the index.
+ double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
+ // We use accurate_index_pos to calculate the position.
+ pos_ = accurate_index_pos + info.encoder - info.index_encoder;
+ }
+}
+
+double ZeroingEstimator::getPosition() { return pos_; }
+
+} // namespace zeroing
+} // namespace frc971