commit 3806ffb21633ca13190a6c4424bdbfad03ad4178
author Austin Schuh <austin.linux@gmail.com> Wed Apr 13 19:44:10 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 13 19:45:33 2022 -0700
tree d8143a0f368b44a8e8b10d1128d5c93e5b6f424b
parent 2cef628ae3343b69ae44f40f5d4cdf7a57ca7357

Make the purple light turn on when we accept images

Drop some of the other colors to make it clearer

Change-Id: I498333523de222888b29d106f276d2f1570e35e6
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2022/control_loops/superstructure/BUILD

diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 31b35fb..f67fa12 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -181,6 +181,7 @@
         "//frc971/queues:gyro_fbs",
         "//third_party:phoenix",
         "//third_party:wpilib",
+        "//y2022/localizer:localizer_output_fbs",
     ],
 )
 

y2022/control_loops/superstructure/led_indicator.cc

diff --git a/y2022/control_loops/superstructure/led_indicator.cc b/y2022/control_loops/superstructure/led_indicator.cc
index 94d0506..b636250 100644
--- a/y2022/control_loops/superstructure/led_indicator.cc
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -17,6 +17,9 @@
       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")) {
@@ -58,12 +61,6 @@
   }
   return false;
 }
-
-bool DrivingFast(
-    const frc971::control_loops::drivetrain::Output &drivetrain_out) {
-  return (drivetrain_out.left_voltage() >= 11.5 ||
-          drivetrain_out.right_voltage() >= 11.5);
-}
 }  // namespace
 
 void LedIndicator::DecideColor() {
@@ -72,6 +69,15 @@
   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() &&
@@ -90,7 +96,8 @@
   // 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) {
+          event_loop_->monotonic_now() -
+              frc971::controls::kLoopFrequency * 10) {
     if (imu_flash_) {
       DisplayLed(255, 0, 0);
     } else {
@@ -122,13 +129,6 @@
     return;
   }
 
-  // Driving fast
-  if (drivetrain_output_fetcher_.get() &&
-      DrivingFast(*drivetrain_output_fetcher_)) {
-    DisplayLed(138, 43, 226);
-    return;
-  }
-
   // Statemachine
   if (superstructure_status_fetcher_.get()) {
     switch (superstructure_status_fetcher_->state()) {
@@ -147,18 +147,16 @@
         } else if (superstructure_status_fetcher_->front_intake_has_ball() ||
                    superstructure_status_fetcher_->back_intake_has_ball()) {
           DisplayLed(165, 42, 42);
-        } else {
-          DisplayLed(0, 255, 0);
         }
         break;
       case (SuperstructureState::SHOOTING):
-        if (!superstructure_status_fetcher_->flippers_open()) {
-          DisplayLed(255, 105, 180);
-        } else {
-          DisplayLed(0, 255, 255);
-        }
         break;
     }
+
+    if (event_loop_->monotonic_now() <
+        last_accepted_time_ + std::chrono::seconds(2)) {
+      DisplayLed(255, 0, 255);
+    }
     return;
   }
 }

y2022/control_loops/superstructure/led_indicator.h

diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index 0f44788..c058254 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -12,6 +12,7 @@
 #include "frc971/queues/gyro_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
 
 namespace y2022::control_loops::superstructure {
 
@@ -58,8 +59,12 @@
       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_;
 
+  size_t last_accepted_count_ = 0;
+  aos::monotonic_clock::time_point last_accepted_time_ =
+      aos::monotonic_clock::min_time;
   size_t imu_counter_ = 0;
   bool imu_flash_ = false;
   size_t disconnected_counter_ = 0;