Merge changes I266cdfce,Ia9866068
* changes:
Add led_indicator to wpilib_interface
Add led_indicator/candle to y2024
diff --git a/y2024/BUILD b/y2024/BUILD
index 7bbf64f..7568050 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -152,7 +152,7 @@
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//frc971/wpilib:pdp_values_fbs",
- #y2019 stuff shouldn't be here (e.g. target selector)
+ # y2019 stuff shouldn't be here (e.g. target selector)
"//y2024/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//y2024/control_loops/superstructure:superstructure_goal_fbs",
@@ -277,6 +277,7 @@
"//third_party:phoenix6",
"//third_party:wpilib",
"//y2024/constants:constants_fbs",
+ "//y2024/control_loops/superstructure:led_indicator_lib",
"//y2024/control_loops/superstructure:superstructure_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_output_fbs",
"//y2024/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
index 6150caa..946579b 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -78,8 +78,7 @@
hdrs = [
"superstructure.h",
],
- data = [
- ],
+ data = [],
deps = [
":collision_avoidance_lib",
":shooter",
@@ -242,3 +241,31 @@
"//aos/network/www:proxy",
],
)
+
+cc_library(
+ name = "led_indicator_lib",
+ srcs = ["led_indicator.cc"],
+ hdrs = ["led_indicator.h"],
+ data = [
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/queues:gyro_fbs",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ ],
+)
diff --git a/y2024/control_loops/superstructure/led_indicator.cc b/y2024/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..3927548
--- /dev/null
+++ b/y2024/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,165 @@
+#include "y2024/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+namespace chrono = std::chrono;
+
+namespace y2024::control_loops::superstructure {
+
+LedIndicator::LedIndicator(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ drivetrain_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ server_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+ "/roborio/aos")),
+ localizer_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ gyro_reading_fetcher_(
+ event_loop_->MakeFetcher<frc971::sensors::GyroReading>(
+ "/drivetrain")),
+ drivetrain_status_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {
+ led::CANdleConfiguration config;
+ config.statusLedOffWhenActive = true;
+ config.disableWhenLOS = false;
+ config.brightnessScalar = 1.0;
+ candle_.ConfigAllSettings(config, 0);
+
+ event_loop_->AddPhasedLoop([this](int) { DecideColor(); },
+ chrono::milliseconds(20));
+ event_loop_->OnRun(
+ [this]() { startup_time_ = event_loop_->monotonic_now(); });
+}
+
+// This method will be called once per scheduler run
+void LedIndicator::DisplayLed(uint8_t r, uint8_t g, uint8_t b) {
+ candle_.SetLEDs(static_cast<int>(r), static_cast<int>(g),
+ static_cast<int>(b));
+}
+
+bool DisconnectedIMUPiServer(
+ const aos::message_bridge::ServerStatistics &server_statistics) {
+ for (const auto *node_status : *server_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool DisconnectedIMUPiClient(
+ const aos::message_bridge::ClientStatistics &client_statistics) {
+ for (const auto *node_status : *client_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool OrinsDisconnected(
+ const frc971::controls::LocalizerOutput &localizer_output) {
+ if (!localizer_output.all_pis_connected()) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_fetcher_.Fetch();
+ drivetrain_status_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
+ gyro_reading_fetcher_.Fetch();
+ localizer_output_fetcher_.Fetch();
+
+ if (localizer_output_fetcher_.get()) {
+ if (localizer_output_fetcher_->image_accepted_count() !=
+ last_accepted_count_) {
+ last_accepted_count_ = localizer_output_fetcher_->image_accepted_count();
+ last_accepted_time_ = event_loop_->monotonic_now();
+ }
+ }
+
+ // Estopped: Red
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // If the imu gyro readings are not being sent/updated recently. Only do this
+ // after we've been on for a bit.
+ if (event_loop_->context().monotonic_event_time >
+ startup_time_ + chrono::seconds(5) &&
+ (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time +
+ frc971::controls::kLoopFrequency * 10 <
+ event_loop_->monotonic_now() ||
+ !gyro_reading_fetcher_->has_velocity())) {
+ // Flash red/white
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(255, 255, 255);
+ }
+ return;
+ }
+
+ if (localizer_output_fetcher_.get() == nullptr ||
+ server_statistics_fetcher_.get() == nullptr ||
+ client_statistics_fetcher_.get() == nullptr ||
+ OrinsDisconnected(*localizer_output_fetcher_) ||
+ DisconnectedIMUPiServer(*server_statistics_fetcher_) ||
+ DisconnectedIMUPiClient(*client_statistics_fetcher_)) {
+ // Flash red/green
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+
+ return;
+ }
+
+ // Not zeroed: Yellow
+ if ((superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) ||
+ (drivetrain_status_fetcher_.get() &&
+ !drivetrain_status_fetcher_->filters_ready())) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+
+ // Want to know when we have a note.
+ //
+ if (superstructure_status_fetcher_.get()) {
+ // Check if there is a target that is in sight
+ if (event_loop_->monotonic_now() <
+ last_accepted_time_ + chrono::milliseconds(100)) {
+ if (superstructure_status_fetcher_.get() != nullptr &&
+ superstructure_status_fetcher_->shooter()->auto_aiming()) {
+ DisplayLed(0, 0, 255);
+ return;
+ } else {
+ DisplayLed(0, 255, 0);
+ return;
+ }
+ }
+ }
+ DisplayLed(0, 0, 0);
+}
+
+} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/led_indicator.h b/y2024/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..a7d891a
--- /dev/null
+++ b/y2024/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,96 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "ctre/phoenix/led/CANdle.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024::control_loops::superstructure {
+
+class FlashCounter {
+ public:
+ FlashCounter(size_t flash_iterations) : flash_iterations_(flash_iterations) {}
+
+ bool Flash() {
+ if (counter_ % flash_iterations_ == 0) {
+ flash_ = !flash_;
+ }
+ counter_++;
+ return flash_;
+ }
+
+ private:
+ size_t flash_iterations_;
+ size_t counter_ = 0;
+ bool flash_ = false;
+};
+
+class LedIndicator {
+ public:
+ LedIndicator(aos::EventLoop *event_loop);
+
+ // Colors in order of priority:
+ //
+ // Red: estopped
+ // Flash red/white: imu disconnected
+ // Flash red/green: any orin disconnected
+ // Pink: not zeroed
+ //
+ // State machine:
+ // INTAKING: Flash Orange/Off
+ // LOADED: Yellow
+ // MOVING: Flash Yellow/Off
+ // LOADING_CATAPULT: Flash Purple/Off
+ // READY: Green
+ // FIRING: Purple
+ //
+ // HAS A TARGET: Blue
+ // VISION: Flash Blue/Off
+
+ void DecideColor();
+
+ private:
+ static constexpr size_t kFlashIterations = 5;
+
+ void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
+
+ ctre::phoenix::led::CANdle candle_{8, "rio"};
+
+ aos::EventLoop *event_loop_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+ aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+
+ size_t last_accepted_count_ = 0;
+ aos::monotonic_clock::time_point last_accepted_time_ =
+ aos::monotonic_clock::min_time;
+
+ aos::monotonic_clock::time_point startup_time_ =
+ aos::monotonic_clock::min_time;
+
+ FlashCounter flash_counter_{kFlashIterations};
+};
+
+} // namespace y2024::control_loops::superstructure
+
+#endif // Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 803845c..301ab79 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -53,6 +53,7 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024/constants.h"
#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/led_indicator.h"
#include "y2024/control_loops/superstructure/superstructure_can_position_static.h"
#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
@@ -731,6 +732,14 @@
AddLoop(&can_output_event_loop);
+ // Thread 6
+ // Setup led_indicator
+ ::aos::ShmEventLoop led_indicator_event_loop(&config.message());
+ led_indicator_event_loop.set_name("LedIndicator");
+ control_loops::superstructure::LedIndicator led_indicator(
+ &led_indicator_event_loop);
+ AddLoop(&led_indicator_event_loop);
+
RunLoops();
}
};