Add led_indicator to 2023
Signed-off-by: Henry <henry@speiser.net>
Change-Id: Iaed47f39bfdcfaf0bb731cb7c7c58d2b649d45fc
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 2700bbc..6df0830 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -152,6 +152,35 @@
],
)
+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",
+ "//y2023/vision:game_pieces_fbs",
+ ],
+)
+
cc_binary(
name = "superstructure_replay",
srcs = ["superstructure_replay.cc"],
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..68e7f14
--- /dev/null
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,183 @@
+#include "y2023/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+
+namespace y2023::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")),
+ superstructure_position_fetcher_(
+ event_loop_->MakeFetcher<Position>("/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([&](int) { DecideColor(); },
+ std::chrono::milliseconds(20));
+}
+
+// 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));
+}
+
+namespace {
+bool DisconnectedPiServer(
+ const aos::message_bridge::ServerStatistics &server_stats) {
+ for (const auto *pi_server_status : *server_stats.connections()) {
+ if (pi_server_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ pi_server_status->node()->name()->string_view() != "logger") {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool DisconnectedPiClient(
+ const aos::message_bridge::ClientStatistics &client_stats) {
+ for (const auto *pi_client_status : *client_stats.connections()) {
+ if (pi_client_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ pi_client_status->node()->name()->string_view() != "logger") {
+ return true;
+ }
+ }
+ return false;
+}
+} // namespace
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ superstructure_position_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_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
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // Not zeroed
+ if (superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) {
+ DisplayLed(255, 0, 255);
+ return;
+ }
+
+ // If the imu gyro readings are not being sent/updated recently
+ if (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time <
+ event_loop_->monotonic_now() -
+ frc971::controls::kLoopFrequency * 10) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(255, 255, 255);
+ }
+ return;
+ }
+
+ // Pi disconnected
+ if ((server_statistics_fetcher_.get() &&
+ DisconnectedPiServer(*server_statistics_fetcher_)) ||
+ (client_statistics_fetcher_.get() &&
+ DisconnectedPiClient(*client_statistics_fetcher_))) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+
+ return;
+ }
+
+ if (superstructure_status_fetcher_.get()) {
+ // Check if end effector is intaking.
+ if (superstructure_status_fetcher_->end_effector_state() ==
+ EndEffectorState::INTAKING) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(0, 0, 0);
+ }
+
+ return;
+ }
+ // Check if end effector is spitting.
+ if (superstructure_status_fetcher_->end_effector_state() ==
+ EndEffectorState::SPITTING) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(0, 255, 0);
+ } else {
+ DisplayLed(0, 0, 0);
+ }
+
+ return;
+ }
+
+ // Check the if there is a cone in the end effector.
+ if (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+ // Check if the cube beam break is triggered.
+ if (superstructure_status_fetcher_->game_piece() == vision::Class::CUBE) {
+ DisplayLed(138, 43, 226);
+ return;
+ }
+
+ // Check if there is a target that is in sight
+ if (drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(255, 165, 0);
+ return;
+ }
+
+ if (event_loop_->monotonic_now() <
+ last_accepted_time_ + std::chrono::milliseconds(500)) {
+ DisplayLed(0, 0, 255);
+ return;
+ }
+
+ return;
+ }
+}
+
+} // namespace y2023::control_loops::superstructure
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..d88650d
--- /dev/null
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,97 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "ctre/phoenix/led/CANdle.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 "y2023/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
+
+namespace y2023::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
+ // Pink: not zeroed
+ // Flash red/white: imu disconnected
+ // Flash red/green: pi disconnected
+ //
+ // Statemachine:
+ // END EFFECTOR INTAKING:
+ // Flash orange/off
+ // END EFFECTOR SPITTING:
+ // Flash green/off
+ // CONE LOADED:
+ // Yellow
+ // CUBE LOADED:
+ // Purple
+ // HAS A TARGET
+ // Gold
+ // VISION:
+ // Blue
+
+ 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_{0, ""};
+
+ aos::EventLoop *event_loop_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<Position> superstructure_position_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;
+
+ FlashCounter flash_counter_{kFlashIterations};
+};
+
+} // namespace y2023::control_loops::superstructure
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_