Remove global y2016.control_loops.superstructure_queue object
Change-Id: I6cbce87055ead432e53732066b3f2d2f7693df06
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index f2a779a..e94cccd 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -63,7 +63,15 @@
shooter_status_fetcher_(
event_loop->MakeFetcher<
::y2016::control_loops::shooter::ShooterQueue::Status>(
- ".y2016.control_loops.shooter.shooter_queue.status")) {}
+ ".y2016.control_loops.shooter.shooter_queue.status")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2016::control_loops::SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
+ superstructure_goal_sender_(
+ event_loop
+ ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")) {}
constexpr double kDoNotTurnCare = 2.0;
@@ -75,8 +83,7 @@
double roller_power) {
superstructure_goal_ = {intake, shoulder, wrist};
- auto new_superstructure_goal =
- ::y2016::control_loops::superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
new_superstructure_goal->angle_intake = intake;
new_superstructure_goal->angle_shoulder = shoulder;
@@ -277,25 +284,25 @@
}
bool AutonomousActor::IntakeDone() {
- control_loops::superstructure_queue.status.FetchAnother();
+ superstructure_status_fetcher_.Fetch();
constexpr double kProfileError = 1e-5;
constexpr double kEpsilon = 0.15;
- if (control_loops::superstructure_queue.status->state < 12 ||
- control_loops::superstructure_queue.status->state == 16) {
+ if (superstructure_status_fetcher_->state < 12 ||
+ superstructure_status_fetcher_->state == 16) {
LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
- if (::std::abs(control_loops::superstructure_queue.status->intake.goal_angle -
+ if (::std::abs(superstructure_status_fetcher_->intake.goal_angle -
superstructure_goal_.intake) < kProfileError &&
- ::std::abs(control_loops::superstructure_queue.status->intake
+ ::std::abs(superstructure_status_fetcher_->intake
.goal_angular_velocity) < kProfileError) {
LOG(DEBUG, "Profile done.\n");
- if (::std::abs(control_loops::superstructure_queue.status->intake.angle -
+ if (::std::abs(superstructure_status_fetcher_->intake.angle -
superstructure_goal_.intake) < kEpsilon &&
- ::std::abs(control_loops::superstructure_queue.status->intake
+ ::std::abs(superstructure_status_fetcher_->intake
.angular_velocity) < kEpsilon) {
LOG(INFO, "Near goal, done.\n");
return true;
@@ -305,48 +312,47 @@
}
bool AutonomousActor::SuperstructureProfileDone() {
- constexpr double kProfileError = 1e-5;
- return ::std::abs(
- control_loops::superstructure_queue.status->intake.goal_angle -
- superstructure_goal_.intake) < kProfileError &&
- ::std::abs(
- control_loops::superstructure_queue.status->shoulder.goal_angle -
- superstructure_goal_.shoulder) < kProfileError &&
- ::std::abs(
- control_loops::superstructure_queue.status->wrist.goal_angle -
- superstructure_goal_.wrist) < kProfileError &&
- ::std::abs(control_loops::superstructure_queue.status->intake
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(control_loops::superstructure_queue.status->shoulder
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(control_loops::superstructure_queue.status->wrist
- .goal_angular_velocity) < kProfileError;
-}
-
-bool AutonomousActor::SuperstructureDone() {
- control_loops::superstructure_queue.status.FetchAnother();
-
- constexpr double kEpsilon = 0.03;
-
- if (control_loops::superstructure_queue.status->state < 12 ||
- control_loops::superstructure_queue.status->state == 16) {
+ if (superstructure_status_fetcher_->state < 12 ||
+ superstructure_status_fetcher_->state == 16) {
LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
+ constexpr double kProfileError = 1e-5;
+ return ::std::abs(
+ superstructure_status_fetcher_->intake.goal_angle -
+ superstructure_goal_.intake) < kProfileError &&
+ ::std::abs(
+ superstructure_status_fetcher_->shoulder.goal_angle -
+ superstructure_goal_.shoulder) < kProfileError &&
+ ::std::abs(
+ superstructure_status_fetcher_->wrist.goal_angle -
+ superstructure_goal_.wrist) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->intake
+ .goal_angular_velocity) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder
+ .goal_angular_velocity) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->wrist
+ .goal_angular_velocity) < kProfileError;
+}
+
+bool AutonomousActor::SuperstructureDone() {
+ superstructure_status_fetcher_.Fetch();
+
+ constexpr double kEpsilon = 0.03;
if (SuperstructureProfileDone()) {
LOG(DEBUG, "Profile done.\n");
- if (::std::abs(control_loops::superstructure_queue.status->intake.angle -
+ if (::std::abs(superstructure_status_fetcher_->intake.angle -
superstructure_goal_.intake) < (kEpsilon + 0.1) &&
- ::std::abs(control_loops::superstructure_queue.status->shoulder.angle -
+ ::std::abs(superstructure_status_fetcher_->shoulder.angle -
superstructure_goal_.shoulder) < (kEpsilon + 0.05) &&
- ::std::abs(control_loops::superstructure_queue.status->wrist.angle -
+ ::std::abs(superstructure_status_fetcher_->wrist.angle -
superstructure_goal_.wrist) < (kEpsilon + 0.01) &&
- ::std::abs(control_loops::superstructure_queue.status->intake
+ ::std::abs(superstructure_status_fetcher_->intake
.angular_velocity) < (kEpsilon + 0.1) &&
- ::std::abs(control_loops::superstructure_queue.status->shoulder
+ ::std::abs(superstructure_status_fetcher_->shoulder
.angular_velocity) < (kEpsilon + 0.10) &&
- ::std::abs(control_loops::superstructure_queue.status->wrist
+ ::std::abs(superstructure_status_fetcher_->wrist
.angular_velocity) < (kEpsilon + 0.05)) {
LOG(INFO, "Near goal, done.\n");
return true;
@@ -356,50 +362,29 @@
}
void AutonomousActor::WaitForIntake() {
- while (true) {
- if (ShouldCancel()) return;
- if (IntakeDone()) return;
- }
+ WaitUntil(::std::bind(&AutonomousActor::IntakeDone, this));
}
void AutonomousActor::WaitForSuperstructure() {
- while (true) {
- if (ShouldCancel()) return;
- if (SuperstructureDone()) return;
- }
+ WaitUntil(::std::bind(&AutonomousActor::SuperstructureDone, this));
}
void AutonomousActor::WaitForSuperstructureProfile() {
- while (true) {
- if (ShouldCancel()) return;
- control_loops::superstructure_queue.status.FetchAnother();
-
- if (control_loops::superstructure_queue.status->state < 12 ||
- control_loops::superstructure_queue.status->state == 16) {
- LOG(ERROR, "Superstructure no longer running, aborting action\n");
- return;
- }
-
- if (SuperstructureProfileDone()) return;
- }
+ WaitUntil([this]() {
+ superstructure_status_fetcher_.Fetch();
+ return SuperstructureProfileDone();
+ });
}
void AutonomousActor::WaitForSuperstructureLow() {
- while (true) {
- if (ShouldCancel()) return;
- control_loops::superstructure_queue.status.FetchAnother();
+ WaitUntil([this]() {
+ superstructure_status_fetcher_.Fetch();
- if (control_loops::superstructure_queue.status->state < 12 ||
- control_loops::superstructure_queue.status->state == 16) {
- LOG(ERROR, "Superstructure no longer running, aborting action\n");
- return;
- }
- if (SuperstructureProfileDone()) return;
- if (control_loops::superstructure_queue.status->shoulder.angle < 0.1) {
- return;
- }
- }
+ return SuperstructureProfileDone() ||
+ superstructure_status_fetcher_->shoulder.angle < 0.1;
+ });
}
+
void AutonomousActor::BackLongShotLowBarTwoBall() {
LOG(INFO, "Expanding for back long shot\n");
MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55, {7.0, 40.0}, {4.0, 6.0},
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index c4ec7f0..8d7baaa 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -12,6 +12,7 @@
#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/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
namespace y2016 {
@@ -94,6 +95,10 @@
shooter_goal_sender_;
::aos::Fetcher<::y2016::control_loops::shooter::ShooterQueue::Status>
shooter_status_fetcher_;
+ ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
+ ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
};
} // namespace actors
diff --git a/y2016/actors/superstructure_actor.cc b/y2016/actors/superstructure_actor.cc
index f65d05a..26fdd7e 100644
--- a/y2016/actors/superstructure_actor.cc
+++ b/y2016/actors/superstructure_actor.cc
@@ -14,7 +14,15 @@
SuperstructureActor::SuperstructureActor(::aos::EventLoop *event_loop)
: aos::common::actions::ActorBase<actors::SuperstructureActionQueueGroup>(
- event_loop, ".y2016.actors.superstructure_action") {}
+ event_loop, ".y2016.actors.superstructure_action"),
+ superstructure_goal_sender_(
+ event_loop
+ ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2016::control_loops::SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")) {}
bool SuperstructureActor::RunAction(
const actors::SuperstructureActionParams ¶ms) {
@@ -37,8 +45,7 @@
bool unfold_climber) {
superstructure_goal_ = {0, shoulder, shooter};
- auto new_superstructure_goal =
- ::y2016::control_loops::superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
new_superstructure_goal->angle_intake = 0;
new_superstructure_goal->angle_shoulder = shoulder;
@@ -67,25 +74,25 @@
bool SuperstructureActor::SuperstructureProfileDone() {
constexpr double kProfileError = 1e-2;
- return ::std::abs(
- control_loops::superstructure_queue.status->intake.goal_angle -
- superstructure_goal_.intake) < kProfileError &&
+ return ::std::abs(superstructure_status_fetcher_->intake.goal_angle -
+ superstructure_goal_.intake) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder.goal_angle -
+ superstructure_goal_.shoulder) < kProfileError &&
::std::abs(
- control_loops::superstructure_queue.status->shoulder.goal_angle -
- superstructure_goal_.shoulder) < kProfileError &&
- ::std::abs(control_loops::superstructure_queue.status->intake
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(control_loops::superstructure_queue.status->shoulder
- .goal_angular_velocity) < kProfileError;
+ superstructure_status_fetcher_->intake.goal_angular_velocity) <
+ kProfileError &&
+ ::std::abs(
+ superstructure_status_fetcher_->shoulder.goal_angular_velocity) <
+ kProfileError;
}
bool SuperstructureActor::SuperstructureDone() {
- control_loops::superstructure_queue.status.FetchAnother();
+ superstructure_status_fetcher_.Fetch();
// We are no longer running if we are in the zeroing states (below 12), or
// estopped.
- if (control_loops::superstructure_queue.status->state < 12 ||
- control_loops::superstructure_queue.status->state == 16) {
+ if (superstructure_status_fetcher_->state < 12 ||
+ superstructure_status_fetcher_->state == 16) {
LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
@@ -98,10 +105,7 @@
}
void SuperstructureActor::WaitForSuperstructure() {
- while (true) {
- if (ShouldCancel()) return;
- if (SuperstructureDone()) return;
- }
+ WaitUntil(::std::bind(&SuperstructureActor::SuperstructureDone, this));
}
} // namespace actors
diff --git a/y2016/actors/superstructure_actor.h b/y2016/actors/superstructure_actor.h
index be02a33..ab84db5 100644
--- a/y2016/actors/superstructure_actor.h
+++ b/y2016/actors/superstructure_actor.h
@@ -3,9 +3,10 @@
#include <memory>
-#include "aos/actions/actor.h"
#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
#include "y2016/actors/superstructure_action.q.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
namespace y2016 {
namespace actors {
@@ -23,6 +24,11 @@
return Factory(event_loop, ".y2016.actors.superstructure_action");
}
+ private:
+ ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
+ ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
// Internal struct holding superstructure goals sent by autonomous to the
// loop.
struct SuperstructureGoal {
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 1431d23..d784929 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -32,6 +32,7 @@
.frc971.EstimatorState estimator_state;
};
+// Published on ".y2016.control_loops.superstructure_queue"
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -142,5 +143,3 @@
queue Output output;
queue Status status;
};
-
-queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 300986e..d69afcb 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -59,6 +59,10 @@
autonomous_mode_fetcher_(
event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
".frc971.autonomous.auto_mode")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2016::control_loops::SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
cur_raw_data_("no data"),
sample_id_(0),
measure_index_(0),
@@ -91,7 +95,7 @@
int auto_mode_indicator = -1;
autonomous_mode_fetcher_.Fetch();
- ::y2016::control_loops::superstructure_queue.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
ball_detector_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
@@ -122,11 +126,11 @@
}
}
- if (::y2016::control_loops::superstructure_queue.status.get()) {
- if (!::y2016::control_loops::superstructure_queue.status->zeroed) {
+ if (superstructure_status_fetcher_.get()) {
+ if (!superstructure_status_fetcher_->zeroed) {
superstructure_state_indicator = superstructure_indicator::kNotZeroed;
}
- if (::y2016::control_loops::superstructure_queue.status->estopped) {
+ if (superstructure_status_fetcher_->estopped) {
superstructure_state_indicator = superstructure_indicator::kEstopped;
}
}
diff --git a/y2016/dashboard/dashboard.h b/y2016/dashboard/dashboard.h
index 58c9890..ed5a579 100644
--- a/y2016/dashboard/dashboard.h
+++ b/y2016/dashboard/dashboard.h
@@ -18,6 +18,7 @@
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
#include "frc971/autonomous/auto.q.h"
+#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
@@ -70,6 +71,8 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
::aos::Fetcher<::frc971::autonomous::AutonomousMode> autonomous_mode_fetcher_;
+ ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ superstructure_status_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 61b6a42..618b538 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::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
@@ -84,6 +83,14 @@
event_loop->MakeSender<
::y2016::control_loops::shooter::ShooterQueue::Goal>(
".y2016.control_loops.shooter.shooter_queue.goal")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2016::control_loops::SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
+ superstructure_goal_sender_(
+ event_loop
+ ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
@@ -148,13 +155,13 @@
LOG(DEBUG, "Canceling\n");
}
- superstructure_queue.status.FetchLatest();
- if (!superstructure_queue.status.get()) {
+ superstructure_status_fetcher_.Fetch();
+ if (!superstructure_status_fetcher_.get()) {
LOG(ERROR, "Got no superstructure status packet.\n");
}
- if (superstructure_queue.status.get() &&
- superstructure_queue.status->zeroed) {
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->zeroed) {
if (waiting_for_zero_) {
LOG(DEBUG, "Zeroed! Starting teleop mode.\n");
waiting_for_zero_ = false;
@@ -315,7 +322,8 @@
if (!waiting_for_zero_) {
if (!is_expanding_) {
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal =
+ superstructure_goal_sender_.MakeMessage();
new_superstructure_goal->angle_intake = intake_goal_;
new_superstructure_goal->angle_shoulder = shoulder_goal_;
new_superstructure_goal->angle_wrist = wrist_goal_;
@@ -387,6 +395,10 @@
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
shooter_goal_sender_;
+ ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
+ ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ superstructure_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 d2d299f..c2a4271 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -54,10 +54,10 @@
#include "y2016/queues/ball_detector.q.h"
using ::frc971::control_loops::drivetrain_queue;
-using ::y2016::control_loops::superstructure_queue;
using aos::make_unique;
using ::frc971::wpilib::LoopOutputHandler;
using ::y2016::control_loops::shooter::ShooterQueue;
+using ::y2016::control_loops::SuperstructureQueue;
namespace y2016 {
namespace wpilib {
@@ -155,7 +155,10 @@
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
".frc971.autonomous.auto_mode")),
shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
- ".y2016.control_loops.shooter.shooter_queue.position")) {
+ ".y2016.control_loops.shooter.shooter_queue.position")),
+ superstructure_position_sender_(
+ event_loop->MakeSender<SuperstructureQueue::Position>(
+ ".y2016.control_loops.superstructure_queue.position")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -283,7 +286,8 @@
}
{
- auto superstructure_message = superstructure_queue.position.MakeMessage();
+ auto superstructure_message =
+ superstructure_position_sender_.MakeMessage();
CopyPosition(intake_encoder_, &superstructure_message->intake,
intake_translate, intake_pot_translate, false,
values.intake.pot_offset);
@@ -321,6 +325,7 @@
::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
drivetrain_right_hall_;