Convert BallDetector to event loops.
Change-Id: Ic2716e5cc6d2573835aa039ce50d1ed96c882eed
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 55afeaf..74a1282 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -52,7 +52,10 @@
actors::VisionAlignActor::MakeFactory(event_loop)),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")) {}
+ ".y2016.vision.vision_status")),
+ ball_detector_fetcher_(
+ event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
+ ".y2016.sensors.ball_detector")) {}
constexpr double kDoNotTurnCare = 2.0;
@@ -538,9 +541,9 @@
}
void AutonomousActor::CloseIfBall() {
- ::y2016::sensors::ball_detector.FetchLatest();
- if (::y2016::sensors::ball_detector.get()) {
- const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ ball_detector_fetcher_.Fetch();
+ if (ball_detector_fetcher_.get()) {
+ const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
if (ball_detected) {
CloseShooter();
}
@@ -560,9 +563,9 @@
return;
}
- ::y2016::sensors::ball_detector.FetchLatest();
- if (::y2016::sensors::ball_detector.get()) {
- const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ ball_detector_fetcher_.Fetch();
+ if (ball_detector_fetcher_.get()) {
+ const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
if (ball_detected) {
return;
}
@@ -570,19 +573,6 @@
}
}
-void AutonomousActor::WaitForBall() {
- while (true) {
- ::y2016::sensors::ball_detector.FetchAnother();
- if (::y2016::sensors::ball_detector.get()) {
- const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
- if (ball_detected) {
- return;
- }
- if (ShouldCancel()) return;
- }
- }
-}
-
void AutonomousActor::TwoBallAuto() {
monotonic_clock::time_point start_time = monotonic_clock::now();
OpenShooter();
@@ -603,9 +593,9 @@
// Check if the ball is there.
bool first_ball_there = true;
- ::y2016::sensors::ball_detector.FetchLatest();
- if (::y2016::sensors::ball_detector.get()) {
- const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ ball_detector_fetcher_.Fetch();
+ if (ball_detector_fetcher_.get()) {
+ const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
first_ball_there = ball_detected;
LOG(INFO, "Saw the ball: %d at %f\n", first_ball_there,
::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
@@ -708,9 +698,9 @@
CloseIfBall();
if (!WaitForDriveNear(kDriveBackDistance - 2.3, kDoNotTurnCare)) return;
- ::y2016::sensors::ball_detector.FetchLatest();
- if (::y2016::sensors::ball_detector.get()) {
- const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ ball_detector_fetcher_.Fetch();
+ if (ball_detector_fetcher_.get()) {
+ const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
if (!ball_detected) {
if (!WaitForDriveDone()) return;
LOG(INFO, "Aborting, no ball %f\n",
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index d905a27..22d963d 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2016/actors/vision_align_actor.h"
+#include "y2016/queues/ball_detector.q.h"
namespace y2016 {
namespace actors {
@@ -55,7 +56,6 @@
void BackLongShotTwoBallFinish();
void BackLongShotLowBarTwoBall();
void BackMiddleShot();
- void WaitForBall();
void TuckArm(bool arm_down, bool traverse_down);
void OpenShooter();
void CloseShooter();
@@ -88,6 +88,7 @@
::std::unique_ptr<::aos::common::actions::Action> vision_action_;
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
};
} // namespace actors
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 984bcc3..c3155bf 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -229,6 +229,9 @@
const ::std::string &name)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
name),
+ ball_detector_fetcher_(
+ event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
+ ".y2016.sensors.ball_detector")),
collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
@@ -704,10 +707,10 @@
output->unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
- ::y2016::sensors::ball_detector.FetchLatest();
+ ball_detector_fetcher_.Fetch();
bool ball_detected = false;
- if (::y2016::sensors::ball_detector.get()) {
- ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ if (ball_detector_fetcher_.get()) {
+ ball_detected = ball_detector_fetcher_->voltage > 2.5;
}
// Climber.
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 7215067..610db8e 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -10,6 +10,7 @@
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/superstructure_controls.h"
+#include "y2016/queues/ball_detector.q.h"
namespace y2016 {
namespace control_loops {
@@ -222,6 +223,9 @@
friend class testing::SuperstructureTest_UpperHardstopStartup_Test;
friend class testing::SuperstructureTest_DisabledWhileZeroingHigh_Test;
friend class testing::SuperstructureTest_DisabledWhileZeroingLow_Test;
+
+ ::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
+
Intake intake_;
Arm arm_;
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index d105a10..af6019c 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -54,6 +54,9 @@
: vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
".y2016.vision.vision_status")),
+ ball_detector_fetcher_(
+ event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
+ ".y2016.sensors.ball_detector")),
cur_raw_data_("no data"),
sample_id_(0),
measure_index_(0),
@@ -87,7 +90,7 @@
::frc971::autonomous::auto_mode.FetchLatest();
::y2016::control_loops::superstructure_queue.status.FetchLatest();
- ::y2016::sensors::ball_detector.FetchLatest();
+ ball_detector_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
// Caused glitching with auto-aim at NASA, so be cautious with this until
@@ -108,11 +111,11 @@
// Ball detector comes after vision because we want to prioritize that
// indication.
- if (::y2016::sensors::ball_detector.get()) {
+ if (ball_detector_fetcher_.get()) {
// TODO(comran): Grab detected voltage from joystick_reader. Except this
// value may not change, so it may be worth it to keep it as it is right
// now.
- if (::y2016::sensors::ball_detector->voltage > 2.5) {
+ if (ball_detector_fetcher_->voltage > 2.5) {
big_indicator = big_indicator::kBallIntaked;
}
}
diff --git a/y2016/dashboard/dashboard.h b/y2016/dashboard/dashboard.h
index 8c64348..665d656 100644
--- a/y2016/dashboard/dashboard.h
+++ b/y2016/dashboard/dashboard.h
@@ -17,6 +17,7 @@
#include "aos/events/event-loop.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
+#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
namespace y2016 {
@@ -64,6 +65,7 @@
};
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
// Storage vector that is written and overwritten with data in a FIFO fashion.
::std::vector<SampleItem> sample_items_;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 33244be..bc0e319 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -77,6 +77,9 @@
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
".y2016.vision.vision_status")),
+ ball_detector_fetcher_(
+ event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
+ ".y2016.sensors.ball_detector")),
is_high_gear_(true),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
@@ -286,9 +289,9 @@
}
bool ball_detected = false;
- ::y2016::sensors::ball_detector.FetchLatest();
- if (::y2016::sensors::ball_detector.get()) {
- ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ ball_detector_fetcher_.Fetch();
+ if (ball_detector_fetcher_.get()) {
+ ball_detected = ball_detector_fetcher_->voltage > 2.5;
}
if (data.PosEdge(kIntakeIn)) {
saw_ball_when_started_intaking_ = ball_detected;
@@ -457,6 +460,7 @@
}
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
bool is_high_gear_;
// Whatever these are set to are our default goals to send out after zeroing.
diff --git a/y2016/queues/ball_detector.q b/y2016/queues/ball_detector.q
index 948cc25..9dc686f 100644
--- a/y2016/queues/ball_detector.q
+++ b/y2016/queues/ball_detector.q
@@ -1,5 +1,6 @@
package y2016.sensors;
+// Published on ".y2016.sensors.ball_detector"
message BallDetector {
// Voltage measured by the ball detector sensor.
@@ -10,4 +11,3 @@
double voltage;
};
-queue BallDetector ball_detector;
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 36f8641..6dd9b51 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -55,10 +55,6 @@
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
using ::frc971::control_loops::drivetrain_queue;
using ::y2016::control_loops::shooter::shooter_queue;
using ::y2016::control_loops::superstructure_queue;
@@ -153,7 +149,10 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::SensorReader(event_loop) {
+ : ::frc971::wpilib::SensorReader(event_loop),
+ ball_detector_sender_(
+ event_loop->MakeSender<::y2016::sensors::BallDetector>(
+ ".y2016.sensors.ball_detector")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -293,8 +292,7 @@
}
{
- auto ball_detector_message =
- ::y2016::sensors::ball_detector.MakeMessage();
+ auto ball_detector_message = ball_detector_sender_.MakeMessage();
ball_detector_message->voltage = ball_detector_->GetVoltage();
LOG_STRUCT(DEBUG, "ball detector", *ball_detector_message);
ball_detector_message.Send();
@@ -314,6 +312,8 @@
}
private:
+ ::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
+
::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;