commit e562e3203e69b48e81d905c9a9639f3b2655c3e6
author Henry Speiser <henry@speiser.net> Fri Apr 01 16:09:32 2022 -0700
committer Henry Speiser <henry@speiser.net> Fri Apr 01 16:51:30 2022 -0700
tree 65a1abec1dca3cb9083c87834772a3b6b7a863e2
parent deb1af860ae3e73ebb97f952afe01c26852ba4b8

Add imu disconnected to status led

Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: I69078f82f9f08def59d4177604f26d26d3d5d6e1

y2022/control_loops/superstructure/BUILD

diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index eb713c2..ebd9ace 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -177,6 +177,7 @@
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
         "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+        "//frc971/queues:gyro_fbs",
         "//third_party:phoenix",
         "//third_party:wpilib",
     ],

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 3fed503..f6b0267 100644
--- a/y2022/control_loops/superstructure/led_indicator.cc
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -15,7 +15,10 @@
               "/roborio/aos")),
       client_statistics_fetcher_(
           event_loop->MakeFetcher<aos::message_bridge::ClientStatistics>(
-              "/roborio/aos")) {
+              "/roborio/aos")),
+      gyro_reading_fetcher_(
+          event_loop->MakeFetcher<frc971::sensors::GyroReading>(
+              "/drivetrain")) {
   led::CANdleConfiguration config;
   config.statusLedOffWhenActive = true;
   config.disableWhenLOS = false;
@@ -67,6 +70,7 @@
   server_statistics_fetcher_.Fetch();
   drivetrain_output_fetcher_.Fetch();
   client_statistics_fetcher_.Fetch();
+  gyro_reading_fetcher_.Fetch();
 
   // Estopped
   if (superstructure_status_fetcher_.get() &&
@@ -82,6 +86,23 @@
     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) {
+    if (imu_flash_) {
+      DisplayLed(255, 0, 0);
+    } else {
+      DisplayLed(255, 255, 255);
+    }
+
+    if (imu_counter_ % kFlashIterations == 0) {
+      imu_flash_ = !imu_flash_;
+    }
+    imu_counter_++;
+    return;
+  }
+
   // Pi disconnected
   if ((server_statistics_fetcher_.get() &&
        DisconnectedPiServer(*server_statistics_fetcher_)) ||

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 680b875..355592c 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -9,6 +9,7 @@
 #include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
 #include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/queues/gyro_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_status_generated.h"
 
@@ -22,6 +23,7 @@
   //
   // Red: estopped
   // Yellow: not zeroed
+  // Flash red/white: imu disconnected
   // Flash red/green: pi disconnected
   // Purple: driving fast
   //
@@ -48,6 +50,7 @@
 
   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_;
@@ -55,7 +58,10 @@
       server_statistics_fetcher_;
   aos::Fetcher<aos::message_bridge::ClientStatistics>
       client_statistics_fetcher_;
+  aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
 
+  size_t imu_counter_ = 0;
+  bool imu_flash_ = false;
   size_t disconnected_counter_ = 0;
   bool disconnected_flash_ = false;
 };