Merge "Fix color labels"
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 2df9a33..fbdcfbb 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -115,6 +115,18 @@
                            (-point[3], -point[4], -point[5]))
         cc_file.append("  }));")
         cc_file.append("}")
+    
+    # Matrix of nodes
+    h_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList();")
+
+    cc_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
+    cc_file.append("  ::std::vector<::Eigen::Matrix<double, 2, 1>> points;")
+    for index, point in enumerate(graph_generate.points):
+        cc_file.append(
+            "  points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());" % (
+                numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+    cc_file.append("  return points;")
+    cc_file.append("}")
 
     # Now create the MakeSearchGraph function.
     h_file.append("")
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 1b7c1ce..480715d 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -18,19 +18,23 @@
  public:
   Arm();
 
+  // If true, tune down all the constants for testing.
+  static constexpr bool kGrannyMode() { return true; }
+
   // The operating voltage.
-  static constexpr double kOperatingVoltage() { return 12.0; }
+  static constexpr double kOperatingVoltage() {
+    return kGrannyMode() ? 6.0 : 12.0;
+  }
   static constexpr double kDt() { return 0.00505; }
-  static constexpr double kAlpha0Max() { return 25.0; }
-  static constexpr double kAlpha1Max() { return 25.0; }
-  static constexpr double kVMax() { return 11.0; }
+  static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 25.0; }
+  static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 25.0; }
+  static constexpr double kVMax() { return kGrannyMode() ? 4.0 : 11.5; }
   static constexpr double kPathlessVMax() { return 4.0; }
 
   void Iterate(const uint32_t *unsafe_goal,
                const control_loops::ArmPosition *position,
                double *proximal_output, double *distal_output,
-               bool *release_arm_brake,
-               control_loops::ArmStatus *status);
+               bool *release_arm_brake, control_loops::ArmStatus *status);
 
   void Reset();
 
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 75198fa..d44b6b2 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -160,6 +160,9 @@
     // Value of the beam breaker sensor. This value is true if the beam is
     // broken, false if the beam isn't broken.
     bool claw_beambreak_triggered;
+    // Value of the beambreak sensor detecting when the box has hit the frame
+    // cutout.
+    bool box_back_beambreak_triggered;
   };
 
   message Output {
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 6ccc6f9..a239cc2 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -263,6 +263,14 @@
     right_intake_cube_detector_ = ::std::move(input);
   }
 
+  void set_claw_beambreak(::std::unique_ptr<DigitalInput> input) {
+    claw_beambreak_ = ::std::move(input);
+  }
+
+  void set_box_back_beambreak(::std::unique_ptr<DigitalInput> input) {
+    box_back_beambreak_ = ::std::move(input);
+  }
+
   // Auto mode switches.
   void set_autonomous_mode(int i, ::std::unique_ptr<DigitalInput> sensor) {
     autonomous_modes_.at(i) = ::std::move(sensor);
@@ -461,6 +469,10 @@
       superstructure_message->right_intake.beam_break =
           right_intake_cube_detector_->Get();
 
+      superstructure_message->claw_beambreak_triggered = claw_beambreak_->Get();
+      superstructure_message->box_back_beambreak_triggered =
+          !box_back_beambreak_->Get();
+
       superstructure_message.Send();
     }
 
@@ -537,6 +549,9 @@
   ::std::unique_ptr<DigitalInput> left_intake_cube_detector_,
       right_intake_cube_detector_;
 
+  ::std::unique_ptr<DigitalInput> claw_beambreak_;
+  ::std::unique_ptr<DigitalInput> box_back_beambreak_;
+
   ::std::unique_ptr<DigitalInput> pwm_trigger_;
 
   ::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
@@ -803,8 +818,8 @@
     reader.set_left_intake_spring_angle(make_unique<AnalogInput>(4));
     reader.set_left_intake_cube_detector(make_unique<DigitalInput>(0));
 
-    reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
-    reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
+    reader.set_claw_beambreak(make_unique<DigitalInput>(8));
+    reader.set_box_back_beambreak(make_unique<DigitalInput>(9));
 
     reader.set_pwm_trigger(make_unique<DigitalInput>(25));