Move LocalizerControl queue to event loops
Change-Id: Ic29f2daaf1cba7c7054f1e689c5051f23424fe07
diff --git a/y2019/actors/BUILD b/y2019/actors/BUILD
index ee70772..a622dbf 100644
--- a/y2019/actors/BUILD
+++ b/y2019/actors/BUILD
@@ -25,6 +25,7 @@
"autonomous_actor.h",
],
deps = [
+ "//aos/events:event-loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
@@ -45,6 +46,7 @@
deps = [
":autonomous_action_lib",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//frc971/autonomous:auto_queue",
],
)
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index a565764..6ad8bff 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -16,7 +16,6 @@
namespace y2019 {
namespace actors {
using ::frc971::control_loops::drivetrain_queue;
-using ::frc971::control_loops::drivetrain::localizer_control;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -30,9 +29,14 @@
} // namespace
AutonomousActor::AutonomousActor(
+ ::aos::EventLoop *event_loop,
::frc971::autonomous::AutonomousActionQueueGroup *s)
: frc971::autonomous::BaseAutonomousActor(
- s, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, s, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ ".frc971.control_loops.drivetrain.localizer_control")) {}
bool AutonomousActor::WaitForDriveXGreater(double x) {
LOG(INFO, "Waiting until x > %f\n", x);
@@ -88,7 +92,7 @@
SendSuperstructureGoal();
{
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start on the left l2.
localizer_resetter->x = 1.0;
localizer_resetter->y = 1.35 * turn_scalar;
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index e25085a..941129f 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -10,6 +10,7 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
namespace y2019 {
@@ -25,7 +26,8 @@
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
- explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
+ explicit AutonomousActor(::aos::EventLoop *event_loop,
+ ::frc971::autonomous::AutonomousActionQueueGroup *s);
bool RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
@@ -180,6 +182,9 @@
// Waits until y is within y of zero.
bool WaitForDriveYCloseToZero(double y);
+
+ ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
};
} // namespace actors
diff --git a/y2019/actors/autonomous_actor_main.cc b/y2019/actors/autonomous_actor_main.cc
index 0a77eb9..6fa55f7 100644
--- a/y2019/actors/autonomous_actor_main.cc
+++ b/y2019/actors/autonomous_actor_main.cc
@@ -1,5 +1,6 @@
#include <stdio.h>
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "frc971/autonomous/auto.q.h"
#include "y2019/actors/autonomous_actor.h"
@@ -7,8 +8,9 @@
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
+ ::aos::ShmEventLoop event_loop;
::y2019::actors::AutonomousActor autonomous(
- &::frc971::autonomous::autonomous_action);
+ &event_loop, &::frc971::autonomous::autonomous_action);
autonomous.Run();
::aos::Cleanup();
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 58d1c36..2ae46f9 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -28,7 +28,6 @@
#include "y2019/vision.pb.h"
using ::y2019::control_loops::superstructure::superstructure_queue;
-using ::frc971::control_loops::drivetrain::localizer_control;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -144,7 +143,11 @@
target_selector_hint_sender_(
event_loop->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")) {
+ ".y2019.control_loops.drivetrain.target_selector_hint")),
+ localizer_control_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ ".frc971.control_loops.drivetrain.localizer_control")) {
const uint16_t team = ::aos::network::GetTeamNumber();
superstructure_queue.goal.FetchLatest();
if (superstructure_queue.goal.get()) {
@@ -205,7 +208,7 @@
}
if (data.PosEdge(kResetLocalizerLeft)) {
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start at the left feeder station.
localizer_resetter->x = 0.6;
localizer_resetter->y = 3.4;
@@ -217,7 +220,7 @@
}
if (data.PosEdge(kResetLocalizerRight)) {
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start at the left feeder station.
localizer_resetter->x = 0.6;
localizer_resetter->y = -3.4;
@@ -229,7 +232,7 @@
}
if (data.PosEdge(kResetLocalizerLeftForwards)) {
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start at the left feeder station.
localizer_resetter->x = 0.4;
localizer_resetter->y = 3.4;
@@ -241,7 +244,7 @@
}
if (data.PosEdge(kResetLocalizerLeftBackwards)) {
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start at the left feeder station.
localizer_resetter->x = 0.4;
localizer_resetter->y = 3.4;
@@ -253,7 +256,7 @@
}
if (data.PosEdge(kResetLocalizerRightForwards)) {
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start at the right feeder station.
localizer_resetter->x = 0.4;
localizer_resetter->y = -3.4;
@@ -265,7 +268,7 @@
}
if (data.PosEdge(kResetLocalizerRightBackwards)) {
- auto localizer_resetter = localizer_control.MakeMessage();
+ auto localizer_resetter = localizer_control_sender_.MakeMessage();
// Start at the right feeder station.
localizer_resetter->x = 0.4;
localizer_resetter->y = -3.4;
@@ -501,6 +504,9 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
+
// 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;