Convert control_loops::shooter::shooter_queue to event loops.
Change-Id: Ie11efa85250288b7f7d64ba7ace6ae6bb92a0a90
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index f486d38..f2a779a 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -55,7 +55,15 @@
".y2016.vision.vision_status")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")) {}
+ ".y2016.sensors.ball_detector")),
+ shooter_goal_sender_(
+ event_loop
+ ->MakeSender<::y2016::control_loops::shooter::ShooterQueue::Goal>(
+ ".y2016.control_loops.shooter.shooter_queue.goal")),
+ shooter_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2016::control_loops::shooter::ShooterQueue::Status>(
+ ".y2016.control_loops.shooter.shooter_queue.status")) {}
constexpr double kDoNotTurnCare = 2.0;
@@ -104,12 +112,12 @@
void AutonomousActor::OpenShooter() {
shooter_speed_ = 0.0;
- if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
- .angular_velocity(shooter_speed_)
- .clamp_open(true)
- .push_to_shooter(false)
- .force_lights_on(false)
- .Send()) {
+ auto shooter_goal = shooter_goal_sender_.MakeMessage();
+ shooter_goal->angular_velocity = shooter_speed_;
+ shooter_goal->clamp_open = true;
+ shooter_goal->push_to_shooter = false;
+ shooter_goal->force_lights_on = false;
+ if (!shooter_goal.Send()) {
LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -117,12 +125,13 @@
void AutonomousActor::CloseShooter() {
shooter_speed_ = 0.0;
- if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
- .angular_velocity(shooter_speed_)
- .clamp_open(false)
- .push_to_shooter(false)
- .force_lights_on(false)
- .Send()) {
+ auto shooter_goal = shooter_goal_sender_.MakeMessage();
+ shooter_goal->angular_velocity = shooter_speed_;
+ shooter_goal->clamp_open = false;
+ shooter_goal->push_to_shooter = false;
+ shooter_goal->force_lights_on = false;
+
+ if (!shooter_goal.Send()) {
LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -134,12 +143,13 @@
// hope of a human aligning the robot.
bool force_lights_on = shooter_speed_ > 1.0;
- if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
- .angular_velocity(shooter_speed_)
- .clamp_open(false)
- .push_to_shooter(false)
- .force_lights_on(force_lights_on)
- .Send()) {
+ auto shooter_goal = shooter_goal_sender_.MakeMessage();
+ shooter_goal->angular_velocity = shooter_speed_;
+ shooter_goal->clamp_open = false;
+ shooter_goal->push_to_shooter = false;
+ shooter_goal->force_lights_on = force_lights_on;
+
+ if (!shooter_goal.Send()) {
LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -147,21 +157,22 @@
void AutonomousActor::Shoot() {
uint32_t initial_shots = 0;
- control_loops::shooter::shooter_queue.status.FetchLatest();
- if (control_loops::shooter::shooter_queue.status.get()) {
- initial_shots = control_loops::shooter::shooter_queue.status->shots;
+ shooter_status_fetcher_.Fetch();
+ if (shooter_status_fetcher_.get()) {
+ initial_shots = shooter_status_fetcher_->shots;
}
// In auto, we want to have the lights on whenever possible since we have no
// hope of a human aligning the robot.
bool force_lights_on = shooter_speed_ > 1.0;
- if (!control_loops::shooter::shooter_queue.goal.MakeWithBuilder()
- .angular_velocity(shooter_speed_)
- .clamp_open(false)
- .push_to_shooter(true)
- .force_lights_on(force_lights_on)
- .Send()) {
+ auto shooter_goal = shooter_goal_sender_.MakeMessage();
+ shooter_goal->angular_velocity = shooter_speed_;
+ shooter_goal->clamp_open = false;
+ shooter_goal->push_to_shooter = true;
+ shooter_goal->force_lights_on = force_lights_on;
+
+ if (!shooter_goal.Send()) {
LOG(ERROR, "Sending shooter goal failed.\n");
}
@@ -172,9 +183,9 @@
if (ShouldCancel()) return;
// Wait for the shot count to change so we know when the shot is complete.
- control_loops::shooter::shooter_queue.status.FetchLatest();
- if (control_loops::shooter::shooter_queue.status.get()) {
- if (initial_shots < control_loops::shooter::shooter_queue.status->shots) {
+ shooter_status_fetcher_.Fetch();
+ if (shooter_status_fetcher_.get()) {
+ if (initial_shots < shooter_status_fetcher_->shots) {
return;
}
}
@@ -189,10 +200,10 @@
while (true) {
if (ShouldCancel()) return;
- control_loops::shooter::shooter_queue.status.FetchLatest();
- if (control_loops::shooter::shooter_queue.status.get()) {
- if (control_loops::shooter::shooter_queue.status->left.ready &&
- control_loops::shooter::shooter_queue.status->right.ready) {
+ shooter_status_fetcher_.Fetch();
+ if (shooter_status_fetcher_.get()) {
+ if (shooter_status_fetcher_->left.ready &&
+ shooter_status_fetcher_->right.ready) {
return;
}
}
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index 22d963d..c4ec7f0 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/control_loops/shooter/shooter.q.h"
#include "y2016/queues/ball_detector.q.h"
namespace y2016 {
@@ -89,6 +90,10 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
+ ::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
+ shooter_goal_sender_;
+ ::aos::Fetcher<::y2016::control_loops::shooter::ShooterQueue::Status>
+ shooter_status_fetcher_;
};
} // namespace actors
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index f6f9298..583b762 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -12,6 +12,7 @@
double angular_velocity;
};
+// Published on ".y2016.control_loops.shooter.shooter_queue"
queue_group ShooterQueue {
implements aos.control_loops.ControlLoop;
@@ -78,5 +79,3 @@
queue Output output;
queue Status status;
};
-
-queue_group ShooterQueue shooter_queue;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index aec9c0c..61b6a42 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -26,7 +26,6 @@
#include "y2016/vision/vision.q.h"
using ::frc971::control_loops::drivetrain_queue;
-using ::y2016::control_loops::shooter::shooter_queue;
using ::y2016::control_loops::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
@@ -81,6 +80,10 @@
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
".y2016.sensors.ball_detector")),
+ shooter_goal_sender_(
+ event_loop->MakeSender<
+ ::y2016::control_loops::shooter::ShooterQueue::Goal>(
+ ".y2016.control_loops.shooter.shooter_queue.goal")),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
@@ -366,13 +369,14 @@
}
}
- if (!shooter_queue.goal.MakeWithBuilder()
- .angular_velocity(shooter_velocity_)
- .clamp_open(is_intaking_ || is_outtaking_)
- .push_to_shooter(fire_)
- .force_lights_on(force_lights_on)
- .shooting_forwards(wrist_goal_ > 0)
- .Send()) {
+ auto shooter_message = shooter_goal_sender_.MakeMessage();
+ shooter_message->angular_velocity = shooter_velocity_;
+ shooter_message->clamp_open = is_intaking_ || is_outtaking_;
+ shooter_message->push_to_shooter = fire_;
+ shooter_message->force_lights_on = force_lights_on;
+ shooter_message->shooting_forwards = wrist_goal_ > 0;
+
+ if (!shooter_message.Send()) {
LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -381,6 +385,8 @@
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
+ ::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
+ shooter_goal_sender_;
// Whatever these are set to are our default goals to send out after zeroing.
double intake_goal_;
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 3167587..d2d299f 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -54,7 +54,6 @@
#include "y2016/queues/ball_detector.q.h"
using ::frc971::control_loops::drivetrain_queue;
-using ::y2016::control_loops::shooter::shooter_queue;
using ::y2016::control_loops::superstructure_queue;
using aos::make_unique;
using ::frc971::wpilib::LoopOutputHandler;
@@ -154,7 +153,9 @@
".y2016.sensors.ball_detector")),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")) {
+ ".frc971.autonomous.auto_mode")),
+ shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
+ ".y2016.control_loops.shooter.shooter_queue.position")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -273,7 +274,7 @@
void RunDmaIteration() {
const auto &values = constants::GetValues();
{
- auto shooter_message = shooter_queue.position.MakeMessage();
+ auto shooter_message = shooter_position_sender_.MakeMessage();
shooter_message->theta_left =
shooter_translate(-shooter_left_encoder_->GetRaw());
shooter_message->theta_right =
@@ -319,6 +320,7 @@
private:
::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
drivetrain_right_hall_;