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_;
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index cf69efc..09d2a42 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -12,16 +12,6 @@
bool bottom_back;
};
-// The internal state of the zeroing estimator.
-struct EstimatorState {
- // If true, there has been a fatal error for the estimator.
- bool error;
- // If the joint has seen an index pulse and is zeroed.
- bool zeroed;
- // The estimated position of the joint.
- double position;
-};
-
queue_group FridgeQueue {
implements aos.control_loops.ControlLoop;