Add zeroing state to claw status messages.
Change-Id: I0144cb1455a3de5abca45e071c22f59cd4631e3f
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index b0ee642..7596a75 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -218,15 +218,6 @@
return arm_zeroing_velocity_;
}
-namespace {
-void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
- EstimatorState *state) {
- state->error = estimator.error();
- state->zeroed = estimator.zeroed();
- state->position = estimator.position();
-}
-} // namespace
-
void Fridge::RunIteration(const control_loops::FridgeQueue::Goal *unsafe_goal,
const control_loops::FridgeQueue::Position *position,
control_loops::FridgeQueue::Output *output,
@@ -518,11 +509,13 @@
status->grabbers.bottom_front = false;
status->grabbers.bottom_back = false;
}
- PopulateEstimatorState(left_arm_estimator_, &status->left_arm_state);
- PopulateEstimatorState(right_arm_estimator_, &status->right_arm_state);
- PopulateEstimatorState(left_elevator_estimator_,
+ zeroing::PopulateEstimatorState(left_arm_estimator_,
+ &status->left_arm_state);
+ zeroing::PopulateEstimatorState(right_arm_estimator_,
+ &status->right_arm_state);
+ zeroing::PopulateEstimatorState(left_elevator_estimator_,
&status->left_elevator_state);
- PopulateEstimatorState(right_elevator_estimator_,
+ zeroing::PopulateEstimatorState(right_elevator_estimator_,
&status->right_elevator_state);
status->estopped = (state_ == ESTOP);
status->state = state_;