commit 17e484ec90eb95db8b53daa25f37c6ed26b1a6d6
author Austin Schuh <austin.linux@gmail.com> Sun Mar 11 01:11:36 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 21 20:12:23 2018 -0700
tree 868fcde3955fdc34b0bf734bef0a3b5296ab87af
parent 0aae92411895d4e5385ba22097284d012b714523

Add hanger code.

This adds the goal nodes for the hanger, and the ability to hang.

Change-Id: Ib33c19298cd08e98abf27d9789982d78f8df191e

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 c283f19..f69bab3 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -44,7 +44,8 @@
                   const bool box_back_beambreak_triggered,
                   const bool intake_clear_of_box, double *proximal_output,
                   double *distal_output, bool *release_arm_brake,
-                  bool *claw_closed, control_loops::ArmStatus *status) {
+                  bool *claw_closed, control_loops::ArmStatus *status,
+                  bool suicide) {
   ::Eigen::Matrix<double, 2, 1> Y;
   const bool outputs_disabled =
       ((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -163,6 +164,18 @@
         state_ = State::ESTOP;
       } else if (outputs_disabled) {
         state_ = State::DISABLED;
+      } else if (suicide) {
+        state_ = State::PREP_CLIMB;
+        climb_count_ = 50;
+      }
+      break;
+
+    case State::PREP_CLIMB:
+      --climb_count_;
+      if (climb_count_ <= 0) {
+        state_ = State::ESTOP;
+      } else if (!suicide) {
+        state_ = State::RUNNING;
       }
       break;
 
@@ -171,8 +184,9 @@
       break;
   }
 
-  const bool disable = outputs_disabled ||
-                       (state_ != State::RUNNING && state_ != State::GOTO_PATH);
+  const bool disable = outputs_disabled || (state_ != State::RUNNING &&
+                                            state_ != State::GOTO_PATH &&
+                                            state_ != State::PREP_CLIMB);
   if (disable) {
     close_enough_for_full_power_ = false;
   }
@@ -344,7 +358,11 @@
         -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
     *distal_output = ::std::max(
         -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
-    *release_arm_brake = true;
+    if (state_ != State::PREP_CLIMB) {
+      *release_arm_brake = true;
+    } else {
+      *release_arm_brake = false;
+    }
     *claw_closed = claw_closed_;
   }
 

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 8e1860f..a39b550 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -38,7 +38,8 @@
                const bool box_back_beambreak_triggered,
                const bool intake_clear_of_box, double *proximal_output,
                double *distal_output, bool *release_arm_brake,
-               bool *claw_closed, control_loops::ArmStatus *status);
+               bool *claw_closed, control_loops::ArmStatus *status,
+               bool suicide);
 
   void Reset();
 
@@ -48,6 +49,7 @@
     DISABLED,
     GOTO_PATH,
     RUNNING,
+    PREP_CLIMB,
     ESTOP,
   };
 
@@ -98,6 +100,8 @@
 
   bool close_enough_for_full_power_ = false;
 
+  int32_t climb_count_ = 0;
+
   EKF arm_ekf_;
   TrajectoryFollower follower_;