zeroing: Update start position only on index pulses.
This is per Austin's request:
https://robotics.mvla.net/gerrit/#/c/178/2/frc971/zeroing/zeroing.cc@62
Change-Id: Ic25d1dd24c33f54a7a95b843eb27db56247b8c4e
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 9228331..92f2875 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -10,7 +10,6 @@
const constants::Values::ZeroingConstants& constants) {
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
- index_pulse_count_after_reset_ = 0;
known_index_pos_ = constants.measured_index_position;
start_pos_samples_.reserve(max_sample_count_);
@@ -24,6 +23,19 @@
start_pos_samples_.clear();
zeroed_ = false;
wait_for_index_pulse_ = true;
+ last_used_index_pulse_count_ = 0;
+}
+
+double ZeroingEstimator::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 - known_index_pos_;
+ // We round index_pos to the closest valid value of the index.
+ double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
+ // Now we reverse the first calculation to get the accurate start position.
+ return accurate_index_pos - latched_encoder + known_index_pos_;
}
void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
@@ -32,7 +44,7 @@
// reset and wait for that count to change before we consider ourselves
// zeroed.
if (wait_for_index_pulse_) {
- index_pulse_count_after_reset_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses;
wait_for_index_pulse_ = false;
}
@@ -57,18 +69,15 @@
// 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 == index_pulse_count_after_reset_ ||
- offset_ratio_ready() < 1.0) {
+ if (!zeroed_ && (info.index_pulses == last_used_index_pulse_count_ ||
+ offset_ratio_ready() < 1.0)) {
start_pos_ = start_average;
- } else {
- // 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 + info.latched_encoder - known_index_pos_;
- // We round index_pos to the closest valid value of the index.
- double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
- // Now we reverse the first calculation to get the accurate start position.
- start_pos_ = accurate_index_pos - info.latched_encoder + known_index_pos_;
+ } 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.
+ start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
+ last_used_index_pulse_count_ = info.index_pulses;
// Now that we have an accurate starting position we can consider ourselves
// zeroed.
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 5566533..9fa335a 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -58,6 +58,11 @@
}
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 estimated position.
double pos_;
// The distance between two consecutive index positions.
@@ -77,12 +82,15 @@
// account for the various ways the encoders get mounted into the robot.
double known_index_pos_;
// Flag for triggering logic that takes note of the current index pulse count
- // after a reset. See `index_pulse_count_after_reset_'.
+ // 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.
- uint32_t index_pulse_count_after_reset_;
+ // 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_;
};