Add ball detector and only intake if we have a ball.
Change-Id: Id72dde414d6dcddd8721056efdee3e5f25900a3a
diff --git a/y2016/BUILD b/y2016/BUILD
index ccb1315..57892ea 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -39,6 +39,7 @@
'//frc971/autonomous:auto_queue',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
+ '//y2016/queues:ball_detector',
],
)
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;
diff --git a/y2016/queues/BUILD b/y2016/queues/BUILD
index 3b282fd..81f75d4 100644
--- a/y2016/queues/BUILD
+++ b/y2016/queues/BUILD
@@ -3,6 +3,13 @@
load('/aos/build/queues', 'queue_library')
queue_library(
+ name = 'ball_detector',
+ srcs = [
+ 'ball_detector.q',
+ ],
+)
+
+queue_library(
name = 'profile_params',
srcs = [
'profile_params.q',
diff --git a/y2016/queues/ball_detector.q b/y2016/queues/ball_detector.q
new file mode 100644
index 0000000..948cc25
--- /dev/null
+++ b/y2016/queues/ball_detector.q
@@ -0,0 +1,13 @@
+package y2016.sensors;
+
+message BallDetector {
+ // Voltage measured by the ball detector sensor.
+
+ // Higher voltage means ball is closer to detector, lower voltage means ball
+ // is far from the sensor or not in the robot at all.
+ // TODO(comran): Check to see if our sensor's output corresponds with the
+ // comment above.
+
+ double voltage;
+};
+queue BallDetector ball_detector;
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index bd55575..2cb2937 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -37,5 +37,6 @@
'//y2016/control_loops/drivetrain:polydrivetrain_plants',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
+ '//y2016/queues:ball_detector',
],
)
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));