Kill global .y2019.control_loops.superstructure.superstructure_queue object
Change-Id: Ib75e31cb6717e7421af2f33848f7a77bfb572d15
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index c933181..0f2b04e 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -25,7 +25,15 @@
localizer_control_sender_(
event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")) {}
+ ".frc971.control_loops.drivetrain.localizer_control")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<::y2019::control_loops::superstructure::
+ SuperstructureQueue::Goal>(
+ ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ superstructure_status_fetcher_(event_loop->MakeFetcher<
+ ::y2019::control_loops::superstructure::
+ SuperstructureQueue::Status>(
+ ".y2019.control_loops.superstructure.superstructure_queue.status")) {}
bool AutonomousActor::WaitForDriveXGreater(double x) {
LOG(INFO, "Waiting until x > %f\n", x);
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 13b336d..1992543 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -17,7 +17,6 @@
namespace actors {
using ::frc971::ProfileParameters;
-using ::y2019::control_loops::superstructure::superstructure_queue;
struct ElevatorWristPosition {
double elevator;
@@ -76,7 +75,7 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
new_superstructure_goal->elevator.unsafe_goal = elevator_goal_;
new_superstructure_goal->wrist.unsafe_goal = wrist_goal_;
new_superstructure_goal->intake.unsafe_goal = intake_goal_;
@@ -100,10 +99,10 @@
}
bool IsSucked() {
- superstructure_queue.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
- if (superstructure_queue.status.get()) {
- return superstructure_queue.status->has_piece;
+ if (superstructure_status_fetcher_.get()) {
+ return superstructure_status_fetcher_->has_piece;
}
return false;
}
@@ -139,20 +138,20 @@
}
bool IsSuperstructureDone() {
- superstructure_queue.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
double kElevatorTolerance = 0.01;
double kWristTolerance = 0.05;
- if (superstructure_queue.status.get()) {
+ if (superstructure_status_fetcher_.get()) {
const bool elevator_at_goal =
::std::abs(elevator_goal_ -
- superstructure_queue.status->elevator.position) <
+ superstructure_status_fetcher_->elevator.position) <
kElevatorTolerance;
const bool wrist_at_goal =
::std::abs(wrist_goal_ -
- superstructure_queue.status->wrist.position) <
+ superstructure_status_fetcher_->wrist.position) <
kWristTolerance;
return elevator_at_goal && wrist_at_goal;
@@ -170,8 +169,7 @@
return false;
}
phased_loop.SleepUntilNext();
- superstructure_queue.status.FetchLatest();
- superstructure_queue.goal.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
if (IsSuperstructureDone()) {
return true;
}
@@ -186,6 +184,12 @@
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
+ ::aos::Sender<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
+ ::aos::Fetcher<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
};
} // namespace actors
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 966f766..e656534 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -42,30 +42,30 @@
: target_selector_hint_sender_(
test_event_loop_.MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")) {}
+ ".y2019.control_loops.drivetrain.target_selector_hint")),
+ superstructure_goal_sender_(test_event_loop_.MakeSender<
+ ::y2019::control_loops::superstructure::
+ SuperstructureQueue::Goal>(
+ ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
private:
::aos::testing::TestSharedMemory my_shm_;
protected:
- virtual void TearDown() override {
- ::y2019::control_loops::superstructure::superstructure_queue.goal.Clear();
- }
-
::aos::ShmEventLoop event_loop_;
::aos::ShmEventLoop test_event_loop_;
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
+ ::aos::Sender<::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
};
TEST_P(TargetSelectorParamTest, ExpectReturn) {
TargetSelector selector(&event_loop_);
{
- auto super_goal =
- ::y2019::control_loops::superstructure::superstructure_queue.goal
- .MakeMessage();
+ auto super_goal = superstructure_goal_sender_.MakeMessage();
super_goal->suction.gamepiece_mode = GetParam().ball_mode ? 0 : 1;
ASSERT_TRUE(super_goal.Send());
}
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index f78bd5a..d19c2b8 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -13,6 +13,7 @@
int32_t gamepiece_mode;
};
+// Published on ".y2019.control_loops.superstructure.superstructure_queue"
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -112,5 +113,3 @@
queue Status status;
queue Position position;
};
-
-queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 37dba72..9ee1a3e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -27,7 +27,6 @@
#include "y2019/status_light.q.h"
#include "y2019/vision.pb.h"
-using ::y2019::control_loops::superstructure::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -150,12 +149,30 @@
::frc971::control_loops::drivetrain::LocalizerControl>(
".frc971.control_loops.drivetrain.localizer_control")),
camera_log_sender_(
- event_loop->MakeSender<::y2019::CameraLog>(".y2019.camera_log")) {
+ event_loop->MakeSender<::y2019::CameraLog>(".y2019.camera_log")),
+ superstructure_goal_fetcher_(event_loop->MakeFetcher<
+ ::y2019::control_loops::superstructure::
+ SuperstructureQueue::Goal>(
+ ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ superstructure_goal_sender_(event_loop->MakeSender<
+ ::y2019::control_loops::superstructure::
+ SuperstructureQueue::Goal>(
+ ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ superstructure_position_fetcher_(
+ event_loop->MakeFetcher<::y2019::control_loops::superstructure::
+ SuperstructureQueue::Position>(
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<::y2019::control_loops::superstructure::
+ SuperstructureQueue::Status>(
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "status")) {
const uint16_t team = ::aos::network::GetTeamNumber();
- superstructure_queue.goal.FetchLatest();
- if (superstructure_queue.goal.get()) {
- grab_piece_ = superstructure_queue.goal->suction.grab_piece;
- switch_ball_ = superstructure_queue.goal->suction.gamepiece_mode == 0;
+ superstructure_goal_fetcher_.Fetch();
+ if (superstructure_goal_fetcher_.get()) {
+ grab_piece_ = superstructure_goal_fetcher_->suction.grab_piece;
+ switch_ball_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
}
video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
@@ -170,19 +187,19 @@
void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
::aos::monotonic_clock::time_point monotonic_now =
::aos::monotonic_clock::now();
- superstructure_queue.position.FetchLatest();
- superstructure_queue.status.FetchLatest();
- if (!superstructure_queue.status.get() ||
- !superstructure_queue.position.get()) {
+ superstructure_position_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+ if (!superstructure_status_fetcher_.get() ||
+ !superstructure_position_fetcher_.get()) {
LOG(ERROR, "Got no superstructure status or position packet.\n");
return;
}
- if (!superstructure_queue.status->has_piece) {
+ if (!superstructure_status_fetcher_->has_piece) {
last_not_has_piece_ = monotonic_now;
}
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
{
auto target_hint = target_selector_hint_sender_.MakeMessage();
@@ -285,7 +302,7 @@
if (data.PosEdge(kRelease) &&
monotonic_now >
last_release_button_press_ + ::std::chrono::milliseconds(500)) {
- if (superstructure_queue.status->has_piece) {
+ if (superstructure_status_fetcher_->has_piece) {
release_mode_ = ReleaseButtonMode::kRelease;
} else {
release_mode_ = ReleaseButtonMode::kBallIntake;
@@ -296,7 +313,7 @@
last_release_button_press_ = monotonic_now;
}
- LOG(INFO, "has_piece: %d\n", superstructure_queue.status->has_piece);
+ LOG(INFO, "has_piece: %d\n", superstructure_status_fetcher_->has_piece);
if (data.IsPressed(kSuctionBall)) {
grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
@@ -304,7 +321,7 @@
} else if ((release_mode_ == ReleaseButtonMode::kRelease &&
data.IsPressed(kRelease)) ||
data.IsPressed(kReleaseButtonBoard) ||
- !superstructure_queue.status->has_piece) {
+ !superstructure_status_fetcher_->has_piece) {
grab_piece_ = false;
LOG(INFO, "releasing due to other thing\n");
}
@@ -328,7 +345,7 @@
}
if (switch_ball_) {
- if (superstructure_queue.status->has_piece) {
+ if (superstructure_status_fetcher_->has_piece) {
new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
}
@@ -397,7 +414,7 @@
new_superstructure_goal->intake.unsafe_goal = 0.83;
}
- if (kDoBallIntake && !superstructure_queue.status->has_piece) {
+ if (kDoBallIntake && !superstructure_status_fetcher_->has_piece) {
elevator_wrist_pos_ = kBallIntakePos;
new_superstructure_goal->roller_voltage = 9.0;
grab_piece_ = true;
@@ -430,7 +447,7 @@
new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
}
- if (superstructure_queue.status->stilts.position > 0.1) {
+ if (superstructure_status_fetcher_->stilts.position > 0.1) {
elevator_wrist_pos_ = kClimbPos;
climbed_ = true;
new_superstructure_goal->wrist.profile_params.max_acceleration = 10;
@@ -440,15 +457,17 @@
// If we've been asked to go above deploy and made it up that high, latch
// was_above.
if (new_superstructure_goal->stilts.unsafe_goal > kDeployStiltPosition &&
- superstructure_queue.status->stilts.position >= kDeployStiltPosition) {
+ superstructure_status_fetcher_->stilts.position >=
+ kDeployStiltPosition) {
was_above_ = true;
- } else if ((superstructure_queue.position->platform_left_detect &&
- superstructure_queue.position->platform_right_detect) &&
+ } else if ((superstructure_position_fetcher_->platform_left_detect &&
+ superstructure_position_fetcher_->platform_right_detect) &&
!data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
was_above_ = false;
}
- if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
+ if (superstructure_status_fetcher_->stilts.position >
+ kDeployStiltPosition &&
new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
@@ -503,6 +522,22 @@
::aos::Sender<::y2019::CameraLog> camera_log_sender_;
+ ::aos::Fetcher<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ superstructure_goal_fetcher_;
+
+ ::aos::Sender<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
+
+ ::aos::Fetcher<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Position>
+ superstructure_position_fetcher_;
+ ::aos::Fetcher<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
+
+
// Bool to track if we've been above the deploy position. Once this bool is
// set, don't let the stilts retract until we see the platform.
bool was_above_ = false;