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/BUILD b/frc971/zeroing/BUILD
index 0b8799f..4b3745f 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -36,6 +36,7 @@
'zeroing.h',
],
deps = [
+ ':wrap',
'//frc971/control_loops:queues',
'//frc971:constants',
],
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
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 1872ad0..4d36ca9 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -25,6 +25,7 @@
public:
using Position = PotAndIndexPosition;
using ZeroingConstants = constants::PotAndIndexPulseZeroingConstants;
+ using State = EstimatorState;
PotAndIndexPulseZeroingEstimator(
const constants::PotAndIndexPulseZeroingConstants &constants);
@@ -44,14 +45,13 @@
bool error() const { return error_; }
// Returns true if the logic considers the corresponding mechanism to be
- // zeroed. It return false otherwise. For example, right after a call to
- // Reset() this returns false.
+ // zeroed. It return false otherwise.
bool zeroed() const { return zeroed_; }
// Return the estimated position of the corresponding mechanism. This value
// is in SI units. For example, the estimator for the elevator would return a
// value in meters for the height relative to absolute zero.
- double position() const { return pos_; }
+ double position() const { return position_; }
// Return the estimated starting position of the corresponding mechansim. In
// some contexts we refer to this as the "offset".
@@ -68,12 +68,13 @@
// value returns 0.0. As more samples get added with UpdateEstimate(...) the
// return value starts increasing to 1.0.
double offset_ratio_ready() const {
- return start_pos_samples_.size() / static_cast<double>(max_sample_count_);
+ return start_pos_samples_.size() /
+ static_cast<double>(constants_.average_filter_size);
}
// Returns true if the sample buffer is full.
bool offset_ready() const {
- return start_pos_samples_.size() == max_sample_count_;
+ return start_pos_samples_.size() == constants_.average_filter_size;
}
private:
@@ -82,26 +83,21 @@
double CalculateStartPosition(double start_average,
double latched_encoder) const;
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::PotAndIndexPulseZeroingConstants constants_;
+
// The estimated position.
- double pos_;
+ double position_;
// 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
// sample.
int samples_idx_;
// Last 'max_sample_count_' samples for start positions.
std::vector<double> start_pos_samples_;
- // The number of the last samples of start position to consider in the
- // estimation.
- size_t max_sample_count_;
// The estimated starting position of the mechanism. We also call this the
// 'offset' in some contexts.
double start_pos_;
- // The absolute position of any index pulse on the mechanism. This is used to
- // 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 `last_used_index_pulse_count_'.
bool wait_for_index_pulse_;
@@ -120,9 +116,72 @@
// Stores the position "start_pos" variable the first time the program
// is zeroed.
double first_start_pos_;
- // The fraction of index_diff (possibly greater than 1) after which an error
- // is reported.
- double allowable_encoder_error_;
+};
+
+// Estimates the position with an absolute encoder which also reports
+// incremental counts, and a potentiometer.
+class PotAndAbsEncoderZeroingEstimator {
+ public:
+ using Position = PotAndAbsolutePosition;
+ using ZeroingConstants = constants::PotAndAbsoluteEncoderZeroingConstants;
+ using State = AbsoluteEstimatorState;
+
+ PotAndAbsEncoderZeroingEstimator(
+ const constants::PotAndAbsoluteEncoderZeroingConstants &constants);
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset();
+
+ // Updates the sensor values for the zeroing logic.
+ void UpdateEstimate(const PotAndAbsolutePosition &info);
+
+ // Returns true if the mechanism is zeroed, and false if it isn't.
+ bool zeroed() const { return zeroed_; }
+
+ // Return the estimated position of the corresponding mechanism. This value
+ // is in SI units. For example, the estimator for the elevator would return a
+ // value in meters for the height relative to absolute zero.
+ double position() const { return position_; }
+
+ // Return the estimated starting position of the corresponding mechansim. In
+ // some contexts we refer to this as the "offset".
+ double offset() const { return offset_; }
+
+ // Returns true if an error has occurred, false otherwise. This gets reset to
+ // false when the Reset() function is called.
+ // TODO(austin): Actually implement this.
+ bool error() const { return false; }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const {
+ return relative_to_absolute_offset_samples_.size() ==
+ constants_.average_filter_size &&
+ offset_samples_.size() == constants_.average_filter_size;
+ }
+
+ // 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_; }
+
+ private:
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::PotAndAbsoluteEncoderZeroingConstants constants_;
+ // True if the mechanism is zeroed.
+ bool zeroed_;
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+ // Offset between the Pot and Relative encoder position.
+ ::std::vector<double> offset_samples_;
+ // Estimated start position of the mechanism
+ double offset_;
+ // The next position in 'relative_to_absolute_offset_samples_' and
+ // 'encoder_samples_' to be used to store the next sample.
+ int samples_idx_;
+ // The unzeroed filtered position.
+ double filtered_position_ = 0.0;
+ // The filtered position.
+ double position_ = 0.0;
};
// Populates an EstimatorState struct with information from the zeroing
@@ -130,6 +189,9 @@
void PopulateEstimatorState(const PotAndIndexPulseZeroingEstimator &estimator,
EstimatorState *state);
+void PopulateEstimatorState(const PotAndAbsEncoderZeroingEstimator &estimator,
+ AbsoluteEstimatorState *state);
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 4cfb5e0..87debdd 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -17,6 +17,7 @@
using control_loops::PositionSensorSimulator;
using constants::PotAndIndexPulseZeroingConstants;
+using constants::PotAndAbsoluteEncoderZeroingConstants;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -35,6 +36,15 @@
estimator->UpdateEstimate(sensor_values);
}
+ void MoveTo(PositionSensorSimulator *simulator,
+ PotAndAbsEncoderZeroingEstimator *estimator, double new_position) {
+ PotAndAbsolutePosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
+ // TODO(phil): Add new MoveTo overloads for different zeroing estimators.
+
::aos::testing::TestSharedMemory my_shm_;
};
@@ -281,13 +291,13 @@
TEST_F(ZeroingTest, TestOffsetError) {
const double index_diff = 0.8;
const double known_index_pos = 2 * index_diff;
- int sample_size = 30;
+ const size_t sample_size = 30;
PositionSensorSimulator sim(index_diff);
sim.Initialize(10 * index_diff, index_diff / 3.0, known_index_pos);
PotAndIndexPulseZeroingEstimator estimator(PotAndIndexPulseZeroingConstants{
sample_size, index_diff, known_index_pos, kIndexErrorFraction});
- for (int i = 0; i < sample_size; i++) {
+ for (size_t i = 0; i < sample_size; i++) {
MoveTo(&sim, &estimator, 13 * index_diff);
}
MoveTo(&sim, &estimator, 8 * index_diff);
@@ -301,5 +311,56 @@
ASSERT_TRUE(estimator.error());
}
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ PotAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position, 0.1};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ PotAndAbsEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 10 * index_diff;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ PotAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position, 0.1};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ PotAndAbsEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
} // namespace zeroing
} // namespace frc971