commit 1defd267f2e6cc67c72d338bc3bb74b21d055a5f
author Austin Schuh <austin.linux@gmail.com> Sun Apr 03 16:13:32 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Apr 03 16:13:32 2016 -0700
tree e17fd26890f0f06c36f6a6e095a8555c315ea9b3
parent b2c3338de4cc5b610a2cfcd00eb8f06968b3a26a

Taught superstructure to turn rollers off.

Change-Id: Iebf041d095ee1fab1a361524c4cc5268765cab7a

y2016/control_loops/superstructure/BUILD

diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index f633d7b..6a10b61 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,6 +78,7 @@
     '//aos/common/controls:control_loop',
     '//aos/common/util:trapezoid_profile',
     '//aos/common:math',
+    '//y2016/queues:ball_detector',
     '//frc971/control_loops:state_feedback_loop',
     '//frc971/control_loops:simple_capped_state_feedback_loop',
     '//frc971/zeroing',

y2016/control_loops/superstructure/superstructure.cc

diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index e5f0fff..7871732 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -7,6 +7,7 @@
 
 #include "y2016/control_loops/superstructure/integral_intake_plant.h"
 #include "y2016/control_loops/superstructure/integral_arm_plant.h"
+#include "y2016/queues/ball_detector.q.h"
 
 #include "y2016/constants.h"
 
@@ -653,12 +654,23 @@
     output->voltage_top_rollers = 0.0;
     output->voltage_bottom_rollers = 0.0;
     if (unsafe_goal) {
-      output->voltage_top_rollers = ::std::max(
-          -kMaxIntakeTopVoltage,
-          ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
-      output->voltage_bottom_rollers = ::std::max(
-          -kMaxIntakeBottomVoltage,
-          ::std::min(unsafe_goal->voltage_bottom_rollers, kMaxIntakeBottomVoltage));
+      ::y2016::sensors::ball_detector.FetchAnother();
+      bool ball_detected = false;
+      if (::y2016::sensors::ball_detector.get()) {
+        ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+      }
+      if (unsafe_goal->force_intake || !ball_detected) {
+        output->voltage_top_rollers = ::std::max(
+            -kMaxIntakeTopVoltage,
+            ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
+        output->voltage_bottom_rollers =
+            ::std::max(-kMaxIntakeBottomVoltage,
+                       ::std::min(unsafe_goal->voltage_bottom_rollers,
+                                  kMaxIntakeBottomVoltage));
+      } else {
+        output->voltage_top_rollers = 0.0;
+        output->voltage_bottom_rollers = 0.0;
+      }
       output->traverse_unlatched = unsafe_goal->traverse_unlatched;
       output->traverse_down = unsafe_goal->traverse_down;
     }

y2016/control_loops/superstructure/superstructure.q

diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index f550512..2919398 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -65,6 +65,8 @@
     float voltage_top_rollers;
     float voltage_bottom_rollers;
 
+    bool force_intake;
+
     // If true, release the latch which holds the traverse mechanism in the
     // middle.
     bool traverse_unlatched;