commit ba9cbba33d615504316157ec39965bf7492d0403
author Neil Balch <neilbalch@gmail.com> Fri Apr 06 22:26:38 2018 -0700
committer Brian Silverman <bsilver16384@gmail.com> Wed May 09 19:50:27 2018 -0700
tree b65212d5a34c49f84e251926fba66df66113f9b9
parent 8e69245ff0fd891e9d05c294eb5afccf53d58be7

Improve intaking.

Add a stuck box detector (box intake velocity), and better handling for
boxes coming in sideways.

Change-Id: I5fa4a5b00f7dd1288f8fed33375953bdfe710559

y2018/control_loops/superstructure/arm/arm.cc

diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index d3997ca..d79a293 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -39,7 +39,7 @@
 void Arm::Reset() { state_ = State::UNINITIALIZED; }
 
 void Arm::Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
-                  const control_loops::ArmPosition *position,
+                  bool close_claw, const control_loops::ArmPosition *position,
                   const bool claw_beambreak_triggered,
                   const bool box_back_beambreak_triggered,
                   const bool intake_clear_of_box, double *proximal_output,
@@ -59,9 +59,20 @@
   if (open_claw) {
     claw_closed_ = false;
   }
-  if (outputs_disabled) {
+  if (close_claw) {
     claw_closed_ = true;
   }
+  if (outputs_disabled) {
+    if (claw_closed_count_ == 0) {
+      claw_closed_ = true;
+    } else {
+      --claw_closed_count_;
+    }
+  } else {
+    // Wait this many iterations before closing the claw.  That prevents
+    // brownouts from closing the claw.
+    claw_closed_count_ = 50;
+  }
 
   Y << position->proximal.encoder + proximal_offset_,
       position->distal.encoder + distal_offset_;

y2018/control_loops/superstructure/arm/arm.h

diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index d86b95b..6b2864a 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -35,7 +35,7 @@
   static constexpr double kGotoPathVMax() { return 6.0; }
 
   void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
-               const control_loops::ArmPosition *position,
+               bool close_claw, const control_loops::ArmPosition *position,
                const bool claw_beambreak_triggered,
                const bool box_back_beambreak_triggered,
                const bool intake_clear_of_box, double *proximal_output,
@@ -114,6 +114,8 @@
   // Start at the 0th index.
   uint32_t current_node_ = 0;
 
+  uint32_t claw_closed_count_ = 0;
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };