Propagated zeroing errors to superstructure so it ESTOPs
Change-Id: I0dd424267aa737a09867ebab541f7cdf7dd30523
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index a8953de..0798824 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -54,6 +54,11 @@
void Intake::Correct(PotAndIndexPosition position) {
estimator_.UpdateEstimate(position);
+ if (estimator_.error()) {
+ LOG(ERROR, "zeroing error with intake_estimator\n");
+ return;
+ }
+
if (!initialized_) {
if (estimator_.offset_ready()) {
UpdateIntakeOffset(estimator_.offset());
@@ -214,6 +219,16 @@
shoulder_estimator_.UpdateEstimate(position_shoulder);
wrist_estimator_.UpdateEstimate(position_wrist);
+ // Handle zeroing errors
+ if (shoulder_estimator_.error()) {
+ LOG(ERROR, "zeroing error with shoulder_estimator\n");
+ return;
+ }
+ if (wrist_estimator_.error()) {
+ LOG(ERROR, "zeroing error with wrist_estimator\n");
+ return;
+ }
+
if (!initialized_) {
if (shoulder_estimator_.offset_ready() && wrist_estimator_.offset_ready()) {
UpdateShoulderOffset(shoulder_estimator_.offset());