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
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index 987bb5e..25b5b9d 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -281,8 +281,7 @@
status->zeroed = state_ == RUNNING;
status->estopped = state_ == ESTOP;
status->state = state_;
- ::frc971::zeroing::PopulateEstimatorState(claw_estimator_,
- &status->zeroing_state);
+ status->zeroing_state = claw_estimator_.GetEstimatorState();
status->angle = claw_loop_->X_hat(0, 0);
status->angular_velocity = claw_loop_->X_hat(1, 0);
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index 518d41f..a6acae7 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -711,14 +711,10 @@
status->grabbers.bottom_front = false;
status->grabbers.bottom_back = false;
}
- ::frc971::zeroing::PopulateEstimatorState(left_arm_estimator_,
- &status->left_arm_state);
- ::frc971::zeroing::PopulateEstimatorState(right_arm_estimator_,
- &status->right_arm_state);
- ::frc971::zeroing::PopulateEstimatorState(left_elevator_estimator_,
- &status->left_elevator_state);
- ::frc971::zeroing::PopulateEstimatorState(right_elevator_estimator_,
- &status->right_elevator_state);
+ status->left_arm_state = left_arm_estimator_.GetEstimatorState();
+ status->right_arm_state = right_arm_estimator_.GetEstimatorState();
+ status->left_elevator_state = left_elevator_estimator_.GetEstimatorState();
+ status->right_elevator_state = right_elevator_estimator_.GetEstimatorState();
status->estopped = (state_ == ESTOP);
status->state = state_;
last_state_ = state_;
diff --git a/y2016_bot3/control_loops/intake/intake_controls.cc b/y2016_bot3/control_loops/intake/intake_controls.cc
index f5ada8c..7192568 100644
--- a/y2016_bot3/control_loops/intake/intake_controls.cc
+++ b/y2016_bot3/control_loops/intake/intake_controls.cc
@@ -157,10 +157,7 @@
}
EstimatorState IntakeArm::IntakeEstimatorState() {
- EstimatorState estimator_state;
- ::frc971::zeroing::PopulateEstimatorState(estimator_, &estimator_state);
-
- return estimator_state;
+ return estimator_.GetEstimatorState();
}
} // namespace intake