Convert StatusLight to event loops.
Change-Id: Ifc8a497b0f30c0ce1624b005791859eef2d9912f
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index a646ab6..1a2aa5e 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -24,6 +24,7 @@
deps = [
":superstructure_queue",
"//aos/controls:control_loop",
+ "//aos/events:event-loop",
"//frc971/control_loops:queues",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018:constants",
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 9474b6d..d94e34d 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -24,21 +24,12 @@
constexpr double kMaxIntakeRollerVoltage = 12.0;
} // namespace
-void SendColors(float red, float green, float blue) {
- auto new_status_light = status_light.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
-
- if (!new_status_light.Send()) {
- LOG(ERROR, "Failed to send lights.\n");
- }
-}
-
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
name),
+ status_light_sender_(
+ event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
@@ -292,6 +283,17 @@
last_box_distance_ = clipped_box_distance;
}
+void Superstructure::SendColors(float red, float green, float blue) {
+ auto new_status_light = status_light_sender_.MakeMessage();
+ new_status_light->red = red;
+ new_status_light->green = green;
+ new_status_light->blue = blue;
+
+ if (!new_status_light.Send()) {
+ LOG(ERROR, "Failed to send lights.\n");
+ }
+}
+
} // namespace superstructure
} // namespace control_loops
} // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 3d1a6af..8ab6bf3 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -4,10 +4,12 @@
#include <memory>
#include "aos/controls/control_loop.h"
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018/control_loops/superstructure/arm/arm.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/status_light.q.h"
namespace y2018 {
namespace control_loops {
@@ -32,6 +34,11 @@
control_loops::SuperstructureQueue::Status *status) override;
private:
+ // Sends the status light message for the 3 colors provided.
+ void SendColors(float red, float green, float blue);
+
+ ::aos::Sender<::y2018::StatusLight> status_light_sender_;
+
intake::IntakeSide intake_left_;
intake::IntakeSide intake_right_;
arm::Arm arm_;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 89cfaa0..5fa4abe 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -274,7 +274,6 @@
".y2018.control_loops.superstructure.status",
".y2018.control_loops.superstructure.position"),
superstructure_(&event_loop_, ".y2018.control_loops.superstructure") {
- status_light.Clear();
::y2018::vision::vision_status.Clear();
::frc971::control_loops::drivetrain_queue.output.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
diff --git a/y2018/status_light.q b/y2018/status_light.q
index 246744c..90d4eec 100644
--- a/y2018/status_light.q
+++ b/y2018/status_light.q
@@ -6,5 +6,3 @@
float green;
float blue;
};
-
-queue StatusLight status_light;
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 154f9d0..f412846 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -382,10 +382,20 @@
class SolenoidWriter {
public:
- SolenoidWriter(::frc971::wpilib::BufferedPcm *pcm)
- : pcm_(pcm),
- drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
- superstructure_(".y2018.control_loops.superstructure_queue.output") {}
+ SolenoidWriter(::aos::EventLoop *event_loop,
+ ::frc971::wpilib::BufferedPcm *pcm)
+ : event_loop_(event_loop),
+ drivetrain_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ superstructure_fetcher_(
+ event_loop->MakeFetcher<
+ ::y2018::control_loops::SuperstructureQueue::Output>(
+ ".y2018.control_loops.superstructure_queue.output")),
+ status_light_fetcher_(event_loop->MakeFetcher<::y2018::StatusLight>(
+ ".y2018.status_light")),
+ pcm_(pcm) {}
// left drive
// right drive
@@ -435,23 +445,23 @@
}
{
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- left_drivetrain_shifter_->Set(!drivetrain_->left_high);
- right_drivetrain_shifter_->Set(!drivetrain_->right_high);
+ drivetrain_fetcher_.Fetch();
+ if (drivetrain_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
+ left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high);
+ right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high);
}
}
{
- superstructure_.FetchLatest();
- if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ superstructure_fetcher_.Fetch();
+ if (superstructure_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
- claw_->Set(!superstructure_->claw_grabbed);
- arm_brakes_->Set(superstructure_->release_arm_brake);
- hook_->Set(superstructure_->hook_release);
- forks_->Set(superstructure_->forks_release);
+ claw_->Set(!superstructure_fetcher_->claw_grabbed);
+ arm_brakes_->Set(superstructure_fetcher_->release_arm_brake);
+ hook_->Set(superstructure_fetcher_->hook_release);
+ forks_->Set(superstructure_fetcher_->forks_release);
}
}
@@ -463,11 +473,12 @@
LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
- status_light.FetchLatest();
+ status_light_fetcher_.Fetch();
// If we don't have a light request (or it's an old one), we are borked.
// Flash the red light slowly.
- if (!status_light.get() ||
- status_light.Age() > chrono::milliseconds(100)) {
+ if (!status_light_fetcher_.get() ||
+ event_loop_->monotonic_now() >
+ status_light_fetcher_->sent_time + chrono::milliseconds(100)) {
StatusLight color;
color.red = 0.0;
color.green = 0.0;
@@ -490,8 +501,8 @@
LOG_STRUCT(DEBUG, "color", color);
SetColor(color);
} else {
- LOG_STRUCT(DEBUG, "color", *status_light);
- SetColor(*status_light);
+ LOG_STRUCT(DEBUG, "color", *status_light_fetcher_);
+ SetColor(*status_light_fetcher_);
}
}
}
@@ -526,16 +537,19 @@
void Quit() { run_ = false; }
private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_fetcher_;
+ ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Output>
+ superstructure_fetcher_;
+ ::aos::Fetcher<::y2018::StatusLight> status_light_fetcher_;
+
::frc971::wpilib::BufferedPcm *pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
left_drivetrain_shifter_, right_drivetrain_shifter_, claw_, arm_brakes_,
hook_, forks_;
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Queue<::y2018::control_loops::SuperstructureQueue::Output>
- superstructure_;
-
::ctre::phoenix::CANifier canifier_{0};
::std::atomic<bool> run_{true};
@@ -736,8 +750,11 @@
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));
+ // This is a separate event loop because we want to run it at much lower
+ // priority.
+ ::aos::ShmEventLoop solenoid_event_loop;
::frc971::wpilib::BufferedPcm pcm;
- SolenoidWriter solenoid_writer(&pcm);
+ SolenoidWriter solenoid_writer(&solenoid_event_loop, &pcm);
solenoid_writer.set_left_drivetrain_shifter(pcm.MakeSolenoid(0));
solenoid_writer.set_right_drivetrain_shifter(pcm.MakeSolenoid(1));
solenoid_writer.set_claw(pcm.MakeSolenoid(2));