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/wpilib/wpilib_interface.cc

diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 0ebd0e5..7c58f15 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -36,6 +36,7 @@
 #include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.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 "frc971/wpilib/joystick_sender.h"
 #include "frc971/wpilib/loop_output_handler.h"
@@ -243,6 +244,12 @@
     wrist_encoder_.set_index(::std::move(index));
   }
 
+  // Ball detector setter.
+  void set_ball_detector(::std::unique_ptr<AnalogInput> analog) {
+    ball_detector_ = ::std::move(analog);
+  }
+
+
   // All of the DMA-related set_* calls must be made before this, and it doesn't
   // hurt to do all of them.
 
@@ -333,6 +340,14 @@
 
       superstructure_message.Send();
     }
+
+    {
+      auto ball_detector_message =
+          ::y2016::sensors::ball_detector.MakeMessage();
+      ball_detector_message->voltage = ball_detector_->GetVoltage();
+      LOG_STRUCT(DEBUG, "ball detector", *ball_detector_message);
+      ball_detector_message.Send();
+    }
   }
 
   void Quit() { run_ = false; }
@@ -371,6 +386,7 @@
   ::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
   ::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_,
       shoulder_encoder_, wrist_encoder_;
+  ::std::unique_ptr<AnalogInput> ball_detector_;
 
   ::std::atomic<bool> run_{true};
   DigitalGlitchFilter drivetrain_shooter_encoder_filter_, hall_filter_,
@@ -611,7 +627,6 @@
     ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
     SensorReader reader;
 
-    // TODO(constants): Update these input numbers.
     reader.set_drivetrain_left_encoder(make_encoder(5));
     reader.set_drivetrain_right_encoder(make_encoder(6));
     reader.set_drivetrain_left_hall(make_unique<AnalogInput>(5));
@@ -632,6 +647,8 @@
     reader.set_wrist_index(make_unique<DigitalInput>(1));
     reader.set_wrist_potentiometer(make_unique<AnalogInput>(1));
 
+    reader.set_ball_detector(make_unique<AnalogInput>(7));
+
     reader.set_dma(make_unique<DMA>());
     ::std::thread reader_thread(::std::ref(reader));