Pull state out of the zeroing estimators more nicely
I'm eventually going to remove the position() member, but for now Austin
wants this too.
Change-Id: I12b0c01eb5174751bd04fee8fdb770378a87f541
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 9a19201..45ef912 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -98,11 +98,7 @@
// Returns the current internal estimator state for logging.
typename ZeroingEstimator::State EstimatorState(int index) {
- typename ZeroingEstimator::State estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(estimators_[index],
- &estimator_state);
-
- return estimator_state;
+ return estimators_[index].GetEstimatorState();
}
// Sets the maximum voltage that will be commanded by the loop.
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 6dd6f42..70fbb56 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -19,25 +19,6 @@
} // namespace
-void PopulateEstimatorState(
- const zeroing::PotAndIndexPulseZeroingEstimator &estimator,
- EstimatorState *state) {
- state->error = estimator.error();
- state->zeroed = estimator.zeroed();
- state->position = estimator.position();
- 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)
: constants_(constants) {
@@ -152,6 +133,17 @@
filtered_position_ = start_average + info.encoder;
}
+PotAndIndexPulseZeroingEstimator::State
+PotAndIndexPulseZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.position = position_;
+ r.pot_position = filtered_position_;
+ return r;
+}
+
+
PotAndAbsEncoderZeroingEstimator::PotAndAbsEncoderZeroingEstimator(
const constants::PotAndAbsoluteEncoderZeroingConstants &constants)
: constants_(constants) {
@@ -309,6 +301,16 @@
position_ = offset_ + info.encoder;
}
+PotAndAbsEncoderZeroingEstimator::State
+PotAndAbsEncoderZeroingEstimator::GetEstimatorState() const {
+ State r;
+ r.error = error_;
+ r.zeroed = zeroed_;
+ r.position = position_;
+ r.pot_position = filtered_position_;
+ return r;
+}
+
void PulseIndexZeroingEstimator::Reset() {
max_index_position_ = ::std::numeric_limits<double>::lowest();
min_index_position_ = ::std::numeric_limits<double>::max();
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 217a820..53b8e31 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -66,10 +66,6 @@
double offset() const override { return offset_; }
- // 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_; }
-
// Returns a number between 0 and 1 that represents the percentage of the
// samples being used in the moving average filter. A value of 0.0 means that
// no samples are being used. A value of 1.0 means that the filter is using
@@ -86,6 +82,9 @@
return start_pos_samples_.size() == constants_.average_filter_size;
}
+ // Returns information about our current state.
+ State GetEstimatorState() const;
+
private:
// This function calculates the start position given the internal state and
// the provided `latched_encoder' value.
@@ -159,9 +158,8 @@
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_; }
+ // Returns information about our current state.
+ State GetEstimatorState() const;
private:
// The zeroing constants used to describe the configuration of the system.
@@ -262,14 +260,6 @@
double position_;
};
-// Populates an EstimatorState struct with information from the zeroing
-// estimator.
-void PopulateEstimatorState(const PotAndIndexPulseZeroingEstimator &estimator,
- EstimatorState *state);
-
-void PopulateEstimatorState(const PotAndAbsEncoderZeroingEstimator &estimator,
- AbsoluteEstimatorState *state);
-
} // namespace zeroing
} // namespace frc971