Merge Adam's zeroing updates and add TODOs.
This branch merges some of the updates that Adam got around to with
the version that's on master.
- Remove all code relating to the zeroing queue.
- Use the PotAndIndexPosition structure to initialize the zeroing
logic.
- Use the PositionSensorSimulator class to test the zeroing logic
instead of its own internal version.
- Removed the "SimpleStep" test since it was written for a type of
noise that used the 'floor' math function. We assume Gaussian noise
in the potentiometer.
Change-Id: I683c0647242092602eac9b8eff36466f0f28ad21
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index f6eddb5..c2150d5 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -1,4 +1,5 @@
#include "frc971/zeroing/zeroing.h"
+
#include <math.h>
#include <vector>
@@ -7,64 +8,59 @@
ZeroingEstimator::ZeroingEstimator(
const constants::Values::ZeroingConstants& constants) {
- DoInit(constants.index_difference, constants.average_filter_size);
+ index_diff_ = constants.index_difference;
+ max_sample_count_ = constants.average_filter_size;
+
+ start_pos_samples_.reserve(max_sample_count_);
+
+ Reset();
}
-ZeroingEstimator::ZeroingEstimator(double index_difference,
- size_t max_sample_count) {
- DoInit(index_difference, max_sample_count);
-}
-
-void ZeroingEstimator::DoInit(double index_difference,
- size_t max_sample_count) {
- index_diff_ = index_difference;
+void ZeroingEstimator::Reset() {
samples_idx_ = 0;
- max_sample_count_ = max_sample_count;
- start_pos_samples_.reserve(max_sample_count);
+ start_pos_ = 0;
+ start_pos_samples_.clear();
+ zeroed_ = false;
}
void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
- ZeroingInfo zinfo;
- zinfo.pot = info.pot;
- zinfo.encoder = info.encoder;
- zinfo.index_encoder = info.latched_encoder;
- zinfo.index_count = info.index_pulses;
- UpdateEstimate(zinfo);
-}
-
-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;
}
+
+ // Drop the oldest sample when we run this function the next time around.
samples_idx_ = (samples_idx_ + 1) % max_sample_count_;
- double start_average = 0.0;
+ double sample_sum = 0.0;
+
for (size_t i = 0; i < start_pos_samples_.size(); ++i) {
- start_average += start_pos_samples_[i];
+ sample_sum += 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.
- */
- double pos;
- if (info.index_count == 0) {
- pos = start_average + info.encoder;
- zeroed_ = false;
+ 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 (info.index_pulses == 0 || offset_ratio_ready() < 1.0) {
+ start_pos_ = start_average;
} else {
// We calculate an aproximation of the value of the last index position.
- double index_pos = start_average + info.index_encoder;
+ double index_pos = start_average + info.latched_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;
+ // Now we reverse the first calculation to the accurate start position.
+ start_pos_ = accurate_index_pos - info.latched_encoder;
+
+ // Now that we have an accurate starting position we can consider ourselves
+ // zeroed.
zeroed_ = true;
}
- offset_ = pos - info.encoder;
+
+ pos_ = start_pos_ + info.encoder;
}
} // namespace zeroing