Require a couple cycles of being disabled.
The arm goes back to the DISABLED state (and therefore the GOTO_PATH
state afterwords) if it gets disabled due to brownouts. Require a
couple cycles of being disabled before we switch states. The arm can't
move fast enough to be a problem for short brownouts.
Change-Id: I9d579f2912f3555837dbfab3d30782a289158cd1
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 812f091..a8c00cb 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -15,9 +15,15 @@
namespace superstructure {
namespace arm {
+namespace {
+
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+constexpr int kMaxBrownoutCount = 4;
+
+} // namespace
+
Arm::Arm()
: proximal_zeroing_estimator_(constants::GetValues().arm_proximal.zeroing),
distal_zeroing_estimator_(constants::GetValues().arm_distal.zeroing),
@@ -50,6 +56,11 @@
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
(release_arm_brake == nullptr) || (claw_closed == nullptr));
+ if (outputs_disabled) {
+ ++brownout_count_;
+ } else {
+ brownout_count_ = 0;
+ }
uint32_t filtered_goal = 0;
if (unsafe_goal != nullptr) {
@@ -178,7 +189,7 @@
distal_zeroing_estimator_.error()) {
LOG(ERROR, "Zeroing error ESTOP\n");
state_ = State::ESTOP;
- } else if (outputs_disabled) {
+ } else if (outputs_disabled && brownout_count_ > kMaxBrownoutCount) {
state_ = State::DISABLED;
} else if (trajectory_override) {
follower_.SwitchTrajectory(nullptr);