Pot + absolute encoder zeroing class
This class takes a pot, absolute encoder, and relative encoder and uses
them to compute the offset. It doesn't yet detect errors or wait until
stopped.
Change-Id: I74bc2031132974d9f3e2e6ddf93b954384a6ce2c
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 69c4de6..70fa866 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -3,6 +3,8 @@
#include <cmath>
#include <vector>
+#include "frc971/zeroing/wrap.h"
+
namespace frc971 {
namespace zeroing {
@@ -15,13 +17,20 @@
state->pot_position = estimator.filtered_position();
}
+void PopulateEstimatorState(
+ const zeroing::PotAndAbsEncoderZeroingEstimator &estimator,
+ AbsoluteEstimatorState *state) {
+ state->error = estimator.error();
+ state->zeroed = estimator.zeroed();
+
+ state->position = estimator.position();
+ state->pot_position = estimator.filtered_position();
+}
+
PotAndIndexPulseZeroingEstimator::PotAndIndexPulseZeroingEstimator(
- const constants::PotAndIndexPulseZeroingConstants &constants) {
- index_diff_ = constants.index_difference;
- max_sample_count_ = constants.average_filter_size;
- known_index_pos_ = constants.measured_index_position;
- allowable_encoder_error_ = constants.allowable_encoder_error;
- start_pos_samples_.reserve(max_sample_count_);
+ const constants::PotAndIndexPulseZeroingConstants &constants)
+ : constants_(constants) {
+ start_pos_samples_.reserve(constants_.average_filter_size);
Reset();
}
@@ -48,11 +57,14 @@
// 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_;
+ double index_pos =
+ start_average + latched_encoder - constants_.measured_index_position;
// We round index_pos to the closest valid value of the index.
- double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
+ double accurate_index_pos = (round(index_pos / constants_.index_difference)) *
+ constants_.index_difference;
// Now we reverse the first calculation to get the accurate start position.
- return accurate_index_pos - latched_encoder + known_index_pos_;
+ return accurate_index_pos - latched_encoder +
+ constants_.measured_index_position;
}
void PotAndIndexPulseZeroingEstimator::UpdateEstimate(
@@ -66,14 +78,14 @@
wait_for_index_pulse_ = false;
}
- if (start_pos_samples_.size() < max_sample_count_) {
+ if (start_pos_samples_.size() < constants_.average_filter_size) {
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_;
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
double sample_sum = 0.0;
@@ -110,23 +122,128 @@
// zeroed.
zeroed_ = true;
// Throw an error if first_start_pos is bigger/smaller than
- // allowable_encoder_error_ * index_diff + start_pos.
+ // constants_.allowable_encoder_error * index_diff + start_pos.
if (::std::abs(first_start_pos_ - start_pos_) >
- allowable_encoder_error_ * index_diff_) {
+ constants_.allowable_encoder_error * constants_.index_difference) {
if (!error_) {
LOG(ERROR,
"Encoder ticks out of range since last index pulse. first start "
"position: %f recent starting position: %f, allowable error: %f\n",
first_start_pos_, start_pos_,
- allowable_encoder_error_ * index_diff_);
+ constants_.allowable_encoder_error * constants_.index_difference);
error_ = true;
}
}
}
- pos_ = start_pos_ + info.encoder;
+ position_ = start_pos_ + info.encoder;
filtered_position_ = start_average + info.encoder;
}
+PotAndAbsEncoderZeroingEstimator::PotAndAbsEncoderZeroingEstimator(
+ const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void PotAndAbsEncoderZeroingEstimator::Reset() {
+ zeroed_ = false;
+ relative_to_absolute_offset_samples_.clear();
+ offset_samples_.clear();
+}
+
+// So, this needs to be a multistep process. We need to first estimate the
+// offset between the absolute encoder and the relative encoder. That process
+// should get us an absolute number which is off by integer multiples of the
+// distance/rev. In parallel, we can estimate the offset between the pot and
+// encoder. When both estimates have converged, we can then compute the offset
+// in a cycle, and which cycle, which gives us the accurate global offset.
+//
+// It's tricky to compute the offset between the absolute and relative encoder.
+// We need to compute this inside 1 revolution. The easiest way to do this
+// would be to wrap the encoder, subtract the two of them, and then average the
+// result. That will struggle when they are off by PI. Instead, we need to
+// wrap the number to +- PI from the current averaged offset.
+void PotAndAbsEncoderZeroingEstimator::UpdateEstimate(
+ const PotAndAbsolutePosition &info) {
+ // TODO(austin): Only add this sample if the robot is stopped.
+
+ // Compute the sum of all the offset samples.
+ double relative_to_absolute_offset_sum = 0.0;
+ for (size_t i = 0; i < relative_to_absolute_offset_samples_.size(); ++i) {
+ relative_to_absolute_offset_sum += relative_to_absolute_offset_samples_[i];
+ }
+
+ // Compute the average offset between the absolute encoder and relative
+ // encoder. If we have 0 samples, assume it is 0.
+ double average_relative_to_absolute_offset =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? 0.0
+ : relative_to_absolute_offset_sum /
+ relative_to_absolute_offset_samples_.size();
+
+ // Now, compute the nearest absolute encoder value to the offset relative
+ // encoder position.
+ const double adjusted_absolute_encoder =
+ Wrap(info.encoder + average_relative_to_absolute_offset,
+ info.absolute_encoder - constants_.measured_absolute_position,
+ constants_.one_revolution_distance);
+
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - info.encoder;
+
+ // Add the sample and update the average with the new reading.
+ const size_t relative_to_absolute_offset_samples_size =
+ relative_to_absolute_offset_samples_.size();
+ if (relative_to_absolute_offset_samples_size <
+ constants_.average_filter_size) {
+ average_relative_to_absolute_offset =
+ (average_relative_to_absolute_offset *
+ relative_to_absolute_offset_samples_size +
+ relative_to_absolute_offset) /
+ (relative_to_absolute_offset_samples_size + 1);
+
+ relative_to_absolute_offset_samples_.push_back(relative_to_absolute_offset);
+ } else {
+ average_relative_to_absolute_offset -=
+ relative_to_absolute_offset_samples_[samples_idx_] /
+ relative_to_absolute_offset_samples_size;
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset;
+ average_relative_to_absolute_offset +=
+ relative_to_absolute_offset / relative_to_absolute_offset_samples_size;
+ }
+
+ // Now compute the offset between the pot and relative encoder.
+ if (offset_samples_.size() < constants_.average_filter_size) {
+ offset_samples_.push_back(info.pot - info.encoder);
+ } else {
+ offset_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) % constants_.average_filter_size;
+
+ double pot_relative_encoder_offset_sum = 0.0;
+ for (size_t i = 0; i < offset_samples_.size(); ++i) {
+ pot_relative_encoder_offset_sum += offset_samples_[i];
+ }
+ const double pot_relative_encoder_offset =
+ pot_relative_encoder_offset_sum / offset_samples_.size();
+
+ offset_ = Wrap(info.encoder + pot_relative_encoder_offset,
+ average_relative_to_absolute_offset + info.encoder,
+ constants_.one_revolution_distance) -
+ info.encoder;
+ if (offset_ready()) {
+ zeroed_ = true;
+ }
+
+ filtered_position_ = pot_relative_encoder_offset + info.encoder;
+ position_ = offset_ + info.encoder;
+}
+
} // namespace zeroing
} // namespace frc971