Removed global y2018 SuperstructureQueue object
Change-Id: I95a9c7e1a200ea957d0a7285409fc709f773ad71
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index c195fff..5b4bda8 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -46,7 +46,15 @@
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ superstructure_goal_sender_(
+ event_loop
+ ->MakeSender<::y2018::control_loops::SuperstructureQueue::Goal>(
+ ".frc971.control_loops.superstructure_queue.goal")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2018::control_loops::SuperstructureQueue::Status>(
+ ".frc971.control_loops.superstructure_queue.status")) {}
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index a97f0d1..1b20190 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -15,7 +15,6 @@
namespace y2018 {
namespace actors {
-using ::y2018::control_loops::superstructure_queue;
using ::frc971::control_loops::drivetrain_queue;
namespace arm = ::y2018::control_loops::superstructure::arm;
@@ -26,6 +25,7 @@
bool RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+
private:
void Reset() {
roller_voltage_ = 0.0;
@@ -42,6 +42,11 @@
SendSuperstructureGoal();
}
+ ::aos::Sender<::y2018::control_loops::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
+ ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
+
double roller_voltage_ = 0.0;
double left_intake_angle_ = -3.2;
double right_intake_angle_ = -3.2;
@@ -78,7 +83,7 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
new_superstructure_goal->intake.roller_voltage = roller_voltage_;
new_superstructure_goal->intake.left_intake_angle = left_intake_angle_;
new_superstructure_goal->intake.right_intake_angle = right_intake_angle_;
@@ -119,9 +124,10 @@
return false;
}
- superstructure_queue.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get() && superstructure_queue.status.get()) {
+ if (drivetrain_queue.status.get() &&
+ superstructure_status_fetcher_.get()) {
const double left_profile_error =
(initial_drivetrain_.left -
drivetrain_queue.status->profiled_left_position_goal);
@@ -142,12 +148,12 @@
const double distance_to_go = (left_error + right_error) / 2.0;
// Check superstructure first.
- if (superstructure_queue.status->arm.current_node ==
+ if (superstructure_status_fetcher_->arm.current_node ==
arm_goal_position_ &&
- superstructure_queue.status->arm.path_distance_to_go <
+ superstructure_status_fetcher_->arm.path_distance_to_go <
arm_threshold) {
LOG(INFO, "Arm finished first: %f, drivetrain %f distance\n",
- superstructure_queue.status->arm.path_distance_to_go,
+ superstructure_status_fetcher_->arm.path_distance_to_go,
::std::abs(distance_to_go));
return true;
}
@@ -158,7 +164,7 @@
::std::abs(distance_to_go) < drive_threshold + kPositionTolerance) {
LOG(INFO,
"Drivetrain finished first: arm %f, drivetrain %f distance\n",
- superstructure_queue.status->arm.path_distance_to_go,
+ superstructure_status_fetcher_->arm.path_distance_to_go,
::std::abs(distance_to_go));
return true;
}
@@ -176,11 +182,12 @@
return false;
}
- superstructure_queue.status.FetchLatest();
- if (superstructure_queue.status.get()) {
- if (superstructure_queue.status->arm.current_node ==
+ superstructure_status_fetcher_.Fetch();
+ if (superstructure_status_fetcher_.get()) {
+ if (superstructure_status_fetcher_->arm.current_node ==
arm_goal_position_ &&
- superstructure_queue.status->arm.path_distance_to_go < threshold) {
+ superstructure_status_fetcher_->arm.path_distance_to_go <
+ threshold) {
return true;
}
}
@@ -197,9 +204,9 @@
return false;
}
- superstructure_queue.status.FetchLatest();
- if (superstructure_queue.status.get()) {
- if (superstructure_queue.status->arm.grab_state == 4) {
+ superstructure_status_fetcher_.Fetch();
+ if (superstructure_status_fetcher_.get()) {
+ if (superstructure_status_fetcher_->arm.grab_state == 4) {
return true;
}
}
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 19fbc05..049a406 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -120,6 +120,7 @@
double voltage_rollers;
};
+// Published on ".y2018.control_loops.superstructure_queue"
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -219,5 +220,3 @@
queue Status status;
queue Position position;
};
-
-queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 6e1aaa5..eb23196 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -26,7 +26,6 @@
using ::aos::monotonic_clock;
using ::frc971::control_loops::drivetrain_queue;
-using ::y2018::control_loops::superstructure_queue;
using ::aos::events::ProtoTXUdpSocket;
using ::aos::events::RXUdpSocket;
using ::aos::input::driver_station::ButtonLocation;
@@ -94,7 +93,19 @@
::aos::network::GetTeamNumber() == 971
? DrivetrainInputReader::InputType::kPistol
: DrivetrainInputReader::InputType::kSteeringWheel,
- {}) {
+ {}),
+ superstructure_position_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2018::control_loops::SuperstructureQueue::Position>(
+ ".frc971.control_loops.superstructure_queue.position")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2018::control_loops::SuperstructureQueue::Status>(
+ ".frc971.control_loops.superstructure_queue.status")),
+ superstructure_goal_sender_(
+ event_loop
+ ->MakeSender<::y2018::control_loops::SuperstructureQueue::Goal>(
+ ".frc971.control_loops.superstructure_queue.goal")) {
const uint16_t team = ::aos::network::GetTeamNumber();
video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
@@ -102,15 +113,15 @@
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
- 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 packet.\n");
return;
}
- auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
new_superstructure_goal->intake.left_intake_angle = intake_goal_;
new_superstructure_goal->intake.right_intake_angle = intake_goal_;
@@ -133,11 +144,11 @@
if (data.IsPressed(kSmallBox)) {
// Deploy the intake.
- if (superstructure_queue.position->box_back_beambreak_triggered) {
+ if (superstructure_position_fetcher_->box_back_beambreak_triggered) {
intake_goal_ = 0.30;
} else {
if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
- superstructure_queue.position->box_distance < 0.15) {
+ superstructure_position_fetcher_->box_distance < 0.15) {
intake_goal_ = 0.18;
} else {
intake_goal_ = -0.60;
@@ -145,22 +156,22 @@
}
} else if (data.IsPressed(kIntakeClosed)) {
// Deploy the intake.
- if (superstructure_queue.position->box_back_beambreak_triggered) {
+ if (superstructure_position_fetcher_->box_back_beambreak_triggered) {
intake_goal_ = 0.30;
} else {
if (new_superstructure_goal->intake.roller_voltage > 0.1) {
- if (superstructure_queue.position->box_distance < 0.10) {
+ if (superstructure_position_fetcher_->box_distance < 0.10) {
new_superstructure_goal->intake.roller_voltage -= 3.0;
intake_goal_ = 0.22;
- } else if (superstructure_queue.position->box_distance < 0.17) {
+ } else if (superstructure_position_fetcher_->box_distance < 0.17) {
intake_goal_ = 0.13;
- } else if (superstructure_queue.position->box_distance < 0.25) {
+ } else if (superstructure_position_fetcher_->box_distance < 0.25) {
intake_goal_ = 0.05;
} else {
intake_goal_ = -0.10;
}
if (robot_velocity() < -0.1 &&
- superstructure_queue.position->box_distance > 0.15) {
+ superstructure_position_fetcher_->box_distance > 0.15) {
intake_goal_ += 0.10;
}
} else {
@@ -174,7 +185,7 @@
if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
intake_goal_ > 0.0) {
- if (superstructure_queue.position->box_distance < 0.10) {
+ if (superstructure_position_fetcher_->box_distance < 0.10) {
new_superstructure_goal->intake.roller_voltage -= 3.0;
}
new_superstructure_goal->intake.roller_voltage += 3.0;
@@ -183,7 +194,7 @@
// If we are disabled, stay at the node closest to where we start. This
// should remove long motions when enabled.
if (!data.GetControlBit(ControlBit::kEnabled) || never_disabled_) {
- arm_goal_position_ = superstructure_queue.status->arm.current_node;
+ arm_goal_position_ = superstructure_status_fetcher_->arm.current_node;
never_disabled_ = false;
}
@@ -192,8 +203,9 @@
grab_box = true;
}
const bool near_goal =
- superstructure_queue.status->arm.current_node == arm_goal_position_ &&
- superstructure_queue.status->arm.path_distance_to_go < 1e-3;
+ superstructure_status_fetcher_->arm.current_node ==
+ arm_goal_position_ &&
+ superstructure_status_fetcher_->arm.path_distance_to_go < 1e-3;
if (data.IsPressed(kArmStepDown) && near_goal) {
uint32_t *front_point = ::std::find(
front_points_.begin(), front_points_.end(), arm_goal_position_);
@@ -264,7 +276,7 @@
arm_goal_position_ = arm::BackMiddle1BoxIndex();
} else if (data.IsPressed(kArmBackLowBox)) {
arm_goal_position_ = arm::BackLowBoxIndex();
- } else if (data.IsPressed(kArmBackSwitch)) {
+ } else if (data.IsPressed(kArmBackSwitch)) {
arm_goal_position_ = arm::BackSwitchIndex();
} else if (data.IsPressed(kArmAboveHang)) {
if (data.IsPressed(kIntakeIn)) {
@@ -336,6 +348,13 @@
return mode();
}
+ ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Position>
+ superstructure_position_fetcher_;
+ ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Status>
+ superstructure_status_fetcher_;
+ ::aos::Sender<::y2018::control_loops::SuperstructureQueue::Goal>
+ superstructure_goal_sender_;
+
// Current goals to send to the robot.
double intake_goal_ = 0.0;