Added estimator state to status.
Change-Id: I541c772bfd159e5db5a8237aa948358e7d296e5b
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index a76f06a..dae8374 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -187,10 +187,10 @@
elevator_zeroing_velocity_ = kElevatorZeroingVelocity;
}
} else if (elevator_zeroing_velocity_ > 0 &&
- estimated_elevator() > average_elevator + 2 * pulse_width) {
+ estimated_elevator() > average_elevator + 1.1 * pulse_width) {
elevator_zeroing_velocity_ = -kElevatorZeroingVelocity;
} else if (elevator_zeroing_velocity_ < 0 &&
- estimated_elevator() < average_elevator - 2 * pulse_width) {
+ estimated_elevator() < average_elevator - 1.1 * pulse_width) {
elevator_zeroing_velocity_ = kElevatorZeroingVelocity;
}
return elevator_zeroing_velocity_;
@@ -211,15 +211,24 @@
arm_zeroing_velocity_ = kArmZeroingVelocity;
}
} else if (arm_zeroing_velocity_ > 0.0 &&
- estimated_arm() > average_arm + 2.0 * pulse_width) {
+ estimated_arm() > average_arm + 1.1 * pulse_width) {
arm_zeroing_velocity_ = -kArmZeroingVelocity;
} else if (arm_zeroing_velocity_ < 0.0 &&
- estimated_arm() < average_arm - 2.0 * pulse_width) {
+ estimated_arm() < average_arm - 1.1 * pulse_width) {
arm_zeroing_velocity_ = kArmZeroingVelocity;
}
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,
@@ -509,6 +518,12 @@
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_,
+ &status->left_elevator_state);
+ PopulateEstimatorState(right_elevator_estimator_,
+ &status->right_elevator_state);
status->estopped = (state_ == ESTOP);
status->state = state_;
last_state_ = state_;