commit aa0573cb64db0e66ecc624375f95075e62b18aff
author Comran Morshed <comranmorsh@gmail.com> Sat Mar 05 19:05:54 2016 +0000
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 13 11:49:43 2016 -0700
tree b83539882f987d377c8dca0b4cf0687223f5dcf4
parent 7c6375c185b70efd30ee0ee7df83caf9ea70630c

Add ball detector and only intake if we have a ball.

Change-Id: Id72dde414d6dcddd8721056efdee3e5f25900a3a

y2016/joystick_reader.cc

diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 0e15eb0..f31b708 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -14,6 +14,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "y2016/control_loops/shooter/shooter.q.h"
 #include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/queues/ball_detector.q.h"
 
 #include "y2016/constants.h"
 #include "frc971/queues/gyro.q.h"
@@ -172,7 +173,17 @@
       wrist_goal_ = 0.0;
     }
 
-    is_intaking_ = data.IsPressed(kIntakeIn);
+    bool ball_detected = false;
+    ::y2016::sensors::ball_detector.FetchLatest();
+    if (::y2016::sensors::ball_detector.get()) {
+      ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+    }
+    if (data.PosEdge(kIntakeIn)) {
+      saw_ball_when_started_intaking_ = ball_detected;
+    }
+
+    is_intaking_ = data.IsPressed(kIntakeIn) &&
+                   (!ball_detected || saw_ball_when_started_intaking_);
 
     if (data.IsPressed(kFire) && shooter_velocity_ != 0.0) {
       fire_ = true;
@@ -263,6 +274,9 @@
   // If we're waiting for the subsystems to zero.
   bool waiting_for_zero_ = true;
 
+  // If true, the ball was present when the intaking button was pressed.
+  bool saw_ball_when_started_intaking_ = false;
+
   bool is_intaking_ = false;
   bool is_outtaking_ = false;
   bool fire_ = false;