Add zeroing state to claw status messages.
Change-Id: I0144cb1455a3de5abca45e071c22f59cd4631e3f
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 057c323..5b78a80 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -249,6 +249,7 @@
status->zeroed = state_ == RUNNING;
status->estopped = state_ == ESTOP;
status->state = state_;
+ zeroing::PopulateEstimatorState(claw_estimator_, &status->zeroing_state);
status->angle = claw_loop_->X_hat(0, 0);
if (output) {
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 00c38c4..0105441 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -45,6 +45,7 @@
bool estopped;
// The internal state of the claw state machine.
uint32_t state;
+ EstimatorState zeroing_state;
// Angle of wrist joint.
double angle;
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 03929a2..bf2b27a 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -21,6 +21,16 @@
uint32_t index_pulses;
};
+// The internal state of a 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;
+};
+
// A left/right pair of PotAndIndexPositions.
struct PotAndIndexPair {
PotAndIndexPosition left;
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;
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index d45c49f..9b2347d 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -6,6 +6,13 @@
namespace frc971 {
namespace zeroing {
+void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
+ EstimatorState *state) {
+ state->error = estimator.error();
+ state->zeroed = estimator.zeroed();
+ state->position = estimator.position();
+}
+
ZeroingEstimator::ZeroingEstimator(
const constants::Values::ZeroingConstants& constants) {
index_diff_ = constants.index_difference;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 9f9329a..7fa2262 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -104,6 +104,11 @@
bool error_;
};
+// Populates an EstimatorState struct with information from the zeroing
+// estimator.
+void PopulateEstimatorState(const ZeroingEstimator &estimator,
+ EstimatorState *state);
+
} // namespace zeroing
} // namespace frc971