Propagated zeroing errors to superstructure so it ESTOPs
Change-Id: I0dd424267aa737a09867ebab541f7cdf7dd30523
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index cdbceb4..bf4d3b5 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -119,6 +119,10 @@
// LOW_ARM_ZERO works by moving the intake out of the way, lifting the arm up,
// leveling the shooter, and then moving back down.
+ if (arm_.error() || intake_.error()) {
+ state_ = ESTOP;
+ }
+
switch (state_) {
case UNINITIALIZED:
// Wait in the uninitialized state until both the arm and intake are
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 1350a15..a860f09 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -16,6 +16,8 @@
namespace superstructure {
namespace testing {
class SuperstructureTest_DisabledGoalTest_Test;
+class SuperstructureTest_ArmZeroingErrorTest_Test;
+class SuperstructureTest_IntakeZeroingErrorTest_Test;
} // namespace testing
class Superstructure
@@ -107,6 +109,8 @@
private:
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
+ friend class testing::SuperstructureTest_ArmZeroingErrorTest_Test;
+ friend class testing::SuperstructureTest_IntakeZeroingErrorTest_Test;
Intake intake_;
Arm arm_;
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());
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index d20b345..ce50b4b 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -24,6 +24,8 @@
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
bool zeroed() const { return zeroed_; }
+ // Returns whether an error has occured
+ bool error() const { return estimator_.error(); }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position);
@@ -71,6 +73,10 @@
const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { estimator_.TriggerError(); }
+
private:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
@@ -96,6 +102,7 @@
bool zeroed_ = false;
};
+
class Arm {
public:
Arm();
@@ -104,6 +111,10 @@
bool zeroed() const { return shoulder_zeroed_ && wrist_zeroed_; }
bool shoulder_zeroed() const { return shoulder_zeroed_; }
bool wrist_zeroed() const { return wrist_zeroed_; }
+ // Returns whether an error has occured
+ bool error() const {
+ return shoulder_estimator_.error() || wrist_estimator_.error();
+ }
// Updates our estimator with the latest position.
void Correct(::frc971::PotAndIndexPosition position_shoulder,
@@ -159,6 +170,10 @@
const Eigen::Matrix<double, 6, 1> &X_hat() const { return loop_->X_hat(); }
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ // For testing:
+ // Triggers an estimator error.
+ void TriggerEstimatorError() { shoulder_estimator_.TriggerError(); }
+
private:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 6d8c18f..7e5c37a 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -514,6 +514,26 @@
VerifyNearGoal();
}
+// Tests that the zeroing errors in the arm are caught
+TEST_F(SuperstructureTest, ArmZeroingErrorTest) {
+ RunIteration();
+ EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
+ superstructure_.arm_.TriggerEstimatorError();
+ RunIteration();
+
+ EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
+}
+
+// Tests that the zeroing errors in the intake are caught
+TEST_F(SuperstructureTest, IntakeZeroingErrorTest) {
+ RunIteration();
+ EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
+ superstructure_.intake_.TriggerEstimatorError();
+ RunIteration();
+
+ EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops