Set SZSDPS state to ESTOP when zeroing estimator errors
Change-Id: If48ec806365b4f11e4485446bbfe6fee8e19af48
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 635db59..2951f1a 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -72,6 +72,8 @@
// Resets the profiled subsystem and returns to uninitialized
void Reset();
+ void TriggerEstimatorError() { profiled_subsystem_.TriggerEstimatorError(); }
+
enum class State : int32_t {
UNINITIALIZED,
DISABLED_INITIALIZED,
@@ -127,6 +129,10 @@
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
+ if (profiled_subsystem_.error()) {
+ state_ = State::ESTOP;
+ }
+
switch (state_) {
case State::UNINITIALIZED:
if (profiled_subsystem_.initialized()) {
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 66b1ab9..f685e1b 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -498,11 +498,22 @@
this->VerifyNearGoal();
}
+// Tests that the subsystem estops when a zeroing error occurs
+TYPED_TEST_P(IntakeSystemTest, ZeroingErrorTest) {
+ this->RunForTime(chrono::seconds(2), true);
+
+ EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::RUNNING);
+ this->subsystem_.TriggerEstimatorError();
+ this->RunIteration(true, false);
+ EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::ESTOP);
+}
+
REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
SaturationTest, RespectsRange, ZeroTest, ZeroNoGoal,
LowerHardstopStartup, UpperHardstopStartup,
ResetTest, DisabledGoalTest, DisabledZeroTest,
- MinPositionTest, MaxPositionTest, NullGoalTest);
+ MinPositionTest, MaxPositionTest, NullGoalTest,
+ ZeroingErrorTest);
INSTANTIATE_TYPED_TEST_CASE_P(My, IntakeSystemTest, TestTypes);
} // namespace control_loops