Move StatusLight to event loop
Change-Id: I862ea7b42c7cd7373652b96e17310bd56145bbbb
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index c827c46..908718d 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -67,6 +67,8 @@
// if the message was successfully sent, and false otherwise.
bool Send() {
RawSender *sender = &msg_.get_deleter();
+ // TODO(austin): This lets multiple senders reorder messages since time
+ // isn't acquired with a lock held.
msg_->SetTimeToNow();
return sender->Send(
reinterpret_cast<RawSender::SendContext *>(msg_.release()));
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 87afdfa..f43b448 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -27,6 +27,7 @@
":superstructure_queue",
":vacuum",
"//aos/controls:control_loop",
+ "//aos/events:event-loop",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2019:constants",
"//y2019:status_light",
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index b6ad9e8..a4796a8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -1,33 +1,21 @@
#include "y2019/control_loops/superstructure/superstructure.h"
#include "aos/controls/control_loops.q.h"
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-
#include "y2019/status_light.q.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
-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");
- }
-}
-} // namespace
-
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+ status_light_sender_(
+ event_loop->MakeSender<::y2019::StatusLight>(".y2019.status_light")),
elevator_(constants::GetValues().elevator.subsystem_params),
wrist_(constants::GetValues().wrist.subsystem_params),
intake_(constants::GetValues().intake),
@@ -158,6 +146,17 @@
}
}
+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 y2019
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 2df9026..2d51cff 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,11 +2,13 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/controls/control_loop.h"
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/control_loops/superstructure/vacuum.h"
+#include "y2019/status_light.q.h"
namespace y2019 {
namespace control_loops {
@@ -42,6 +44,10 @@
SuperstructureQueue::Status *status) override;
private:
+ void SendColors(float red, float green, float blue);
+
+ ::aos::Sender<::y2019::StatusLight> status_light_sender_;
+
PotAndAbsoluteEncoderSubsystem elevator_;
PotAndAbsoluteEncoderSubsystem wrist_;
AbsoluteEncoderSubsystem intake_;
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index dd327ee..aa81380 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -16,7 +16,6 @@
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
#include "y2019/control_loops/superstructure/superstructure.h"
#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
-#include "y2019/status_light.q.h"
namespace y2019 {
namespace control_loops {
@@ -290,7 +289,6 @@
".y2019.control_loops.superstructure.superstructure_queue."
"position"),
superstructure_(&event_loop_) {
- status_light.Clear();
::frc971::control_loops::drivetrain_queue.status.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
diff --git a/y2019/status_light.q b/y2019/status_light.q
index c26de88..65a121a 100644
--- a/y2019/status_light.q
+++ b/y2019/status_light.q
@@ -1,5 +1,6 @@
package y2019;
+// Published on ".y2019.status_light"
message StatusLight {
// How bright to make each one. 0 is off, 1 is full on.
float red;
@@ -7,8 +8,7 @@
float blue;
};
-queue StatusLight status_light;
-
+// Published on ".y2019.camera_log"
message CameraLog {
bool log;
};
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index e08b4b1..01c8afa 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -526,10 +526,14 @@
class SolenoidWriter {
public:
- SolenoidWriter()
- : superstructure_(
- ".y2019.control_loops.superstructure.superstructure_queue.output") {
- }
+ SolenoidWriter(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ superstructure_fetcher_(event_loop->MakeFetcher<
+ ::y2019::control_loops::superstructure::
+ SuperstructureQueue::Output>(
+ ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ status_light_fetcher_(event_loop->MakeFetcher<::y2019::StatusLight>(
+ ".y2019.status_light")) {}
void set_big_suction_cup(int index0, int index1) {
big_suction_cup0_ = pcm_.MakeSolenoid(index0);
@@ -563,18 +567,20 @@
}
{
- superstructure_.FetchLatest();
- if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ superstructure_fetcher_.Fetch();
+ if (superstructure_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
- big_suction_cup0_->Set(!superstructure_->intake_suction_bottom);
- big_suction_cup1_->Set(!superstructure_->intake_suction_bottom);
- small_suction_cup0_->Set(superstructure_->intake_suction_top);
- small_suction_cup1_->Set(superstructure_->intake_suction_top);
+ big_suction_cup0_->Set(
+ !superstructure_fetcher_->intake_suction_bottom);
+ big_suction_cup1_->Set(
+ !superstructure_fetcher_->intake_suction_bottom);
+ small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top);
+ small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top);
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- ::aos::Clip(superstructure_->intake_roller_voltage,
+ ::aos::Clip(superstructure_fetcher_->intake_roller_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
}
@@ -588,11 +594,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() ||
+ status_light_fetcher_.get()->sent_time + chrono::milliseconds(100) <
+ event_loop_->monotonic_now()) {
StatusLight color;
color.red = 0.0;
color.green = 0.0;
@@ -610,8 +617,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_.get());
+ SetColor(*status_light_fetcher_.get());
}
}
}
@@ -646,6 +653,8 @@
void Quit() { run_ = false; }
private:
+ ::aos::EventLoop *event_loop_;
+
::frc971::wpilib::BufferedPcm pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> big_suction_cup0_,
@@ -654,9 +663,10 @@
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
intake_rollers_talon_;
- ::aos::Queue<
+ ::aos::Fetcher<
::y2019::control_loops::superstructure::SuperstructureQueue::Output>
- superstructure_;
+ superstructure_fetcher_;
+ ::aos::Fetcher<::y2019::StatusLight> status_light_fetcher_;
::ctre::phoenix::CANifier canifier_{0};
@@ -681,6 +691,7 @@
::aos::SetCurrentThreadName("StartCompetition");
::aos::ShmEventLoop event_loop;
+ ::aos::ShmEventLoop solenoid_event_loop;
::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
@@ -762,7 +773,7 @@
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));
- SolenoidWriter solenoid_writer;
+ SolenoidWriter solenoid_writer(&solenoid_event_loop);
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
solenoid_writer.set_big_suction_cup(0, 1);