commit d67e9cf45df9b1f71ef1ab3c5bc3e0c0994225e0
author Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 13 13:56:57 2022 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 13 14:55:33 2022 -0700
tree b77c5ffedc1f40371be2f44a5bf363010e3031f1
parent 2da80bbdd79a136f91fa7c23cf867ceb5e02e026

Add boilerplate localizer code for LED control

Will use this to turn LEDs facing away from the target off

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I4e03778f6b06db87b26ab3986dfebab7840d6de1

y2022/vision/BUILD

diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index bede03a..e9543cf 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -120,6 +120,7 @@
         "//frc971/vision:v4l2_reader",
         "//frc971/vision:vision_fbs",
         "//third_party:opencv",
+        "//y2022/localizer:localizer_output_fbs",
     ],
 )
 

y2022/vision/camera_reader.cc

diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index f1cb4e8..94b2d96 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -211,6 +211,26 @@
 
   reader_->SendLatestImage();
   read_image_timer_->Setup(event_loop_->monotonic_now());
+
+  // Disable the LEDs based on localizer output
+  if (localizer_output_fetcher_.Fetch()) {
+    const auto node_name = event_loop_->node()->name()->string_view();
+    const size_t pi_number =
+        std::atol(node_name.substr(node_name.size() - 1).data());
+
+    CHECK(localizer_output_fetcher_->has_led_outputs() &&
+          localizer_output_fetcher_->led_outputs()->size() > pi_number);
+
+    const LedOutput led_output =
+        localizer_output_fetcher_->led_outputs()->Get(pi_number);
+
+    if (led_output != prev_led_output_) {
+      gpio_disable_control_.GPIOWrite(led_output == LedOutput::OFF ? kGPIOHigh
+                                                                   : kGPIOLow);
+
+      prev_led_output_ = led_output;
+    }
+  }
 }
 
 }  // namespace vision

y2022/vision/camera_reader.h

diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index edc3a1a..707e04e 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -13,6 +13,7 @@
 #include "aos/network/team_number.h"
 #include "frc971/vision/v4l2_reader.h"
 #include "frc971/vision/vision_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
 #include "y2022/vision/calibration_data.h"
 #include "y2022/vision/calibration_generated.h"
 #include "y2022/vision/gpio.h"
@@ -23,6 +24,7 @@
 namespace vision {
 
 using namespace frc971::vision;
+using frc971::controls::LedOutput;
 
 // TODO<jim>: Probably need to break out LED control to separate process
 class CameraReader {
@@ -38,6 +40,9 @@
         target_estimator_(CameraIntrinsics(), CameraExtrinsics()),
         target_estimate_sender_(
             event_loop->MakeSender<TargetEstimate>("/camera")),
+        localizer_output_fetcher_(
+            event_loop->MakeFetcher<frc971::controls::LocalizerOutput>(
+                "/localizer")),
         read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })),
         gpio_imu_pin_(GPIOControl(GPIO_PIN_SCLK_IMU, kGPIOIn)),
         gpio_pwm_control_(GPIOPWMControl(GPIO_PIN_SCK_PWM, duty_cycle_)),
@@ -99,6 +104,9 @@
   TargetEstimator target_estimator_;
   aos::Sender<TargetEstimate> target_estimate_sender_;
 
+  LedOutput prev_led_output_ = LedOutput::ON;
+  aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+
   // We schedule this immediately to read an image. Having it on a timer
   // means other things can run on the event loop in between.
   aos::TimerHandler *const read_image_timer_;