commit 1e2d498672038344b5f1578a8883ec40f7d3424b
author Austin Schuh <austin.linux@gmail.com> Wed Mar 21 20:34:33 2018 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 21 21:26:28 2018 -0700
tree 0f5077de133190ec0641abf4a55068ee89e6c915
parent 8e5950d2e4311f9ca3ec07f831bdcd39bd420e7f

Make light change only when the message contents change.

Change-Id: If81caf2107227ecb527401136ca6f9fa261d52ac

y2018/BUILD

diff --git a/y2018/BUILD b/y2018/BUILD
index 19284e8..67ad583 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -39,6 +39,7 @@
         "joystick_reader.cc",
     ],
     deps = [
+        ":status_light",
         "//aos/common:time",
         "//aos/common/actions:action_lib",
         "//aos/common/logging",

y2018/joystick_reader.cc

diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index a5eaf14..81791e5 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -17,6 +17,7 @@
 #include "y2018/control_loops/drivetrain/drivetrain_base.h"
 #include "y2018/control_loops/superstructure/arm/generated_graph.h"
 #include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/status_light.q.h"
 
 using ::frc971::control_loops::drivetrain_queue;
 using ::y2018::control_loops::superstructure_queue;
@@ -103,6 +104,17 @@
     drivetrain_input_reader_->HandleDrivetrain(data);
   }
 
+  void SendColors(float red, float green, float blue) {
+    auto new_status_light = status_light.MakeMessage();
+    new_status_light->red = red;
+    new_status_light->green = green;
+    new_status_light->blue = blue;
+
+    if (!new_status_light.Send()) {
+      LOG(ERROR, "Failed to send lights.\n");
+    }
+  }
+
   void HandleTeleop(const ::aos::input::driver_station::Data &data) {
     if (!data.GetControlBit(ControlBit::kEnabled)) {
       action_queue_.CancelAllActions();

y2018/wpilib_interface.cc

diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 6fba1fe..4a7fe1d 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -660,17 +660,31 @@
         LOG_STRUCT(DEBUG, "pneumatics info", to_log);
       }
 
+      static double last_red = -1.0;
+      static double last_green = -1.0;
+      static double last_blue = -1.0;
       status_light.FetchLatest();
       if (status_light.get()) {
         LOG_STRUCT(DEBUG, "writing", *status_light);
         // Not sure which of these is red vs green. We're not ready to use
-        // either,
-        // so just turn them off.
-        canifier_.SetLEDOutput(1.0, ::ctre::phoenix::CANifier::LEDChannelA);
-        canifier_.SetLEDOutput(1.0, ::ctre::phoenix::CANifier::LEDChannelB);
-        // Red
-        canifier_.SetLEDOutput(1 - status_light->red,
-                               ::ctre::phoenix::CANifier::LEDChannelC);
+        // either, so just turn them off.
+        if (status_light->green != last_green) {
+          canifier_.SetLEDOutput(1.0 - status_light->green,
+                                 ::ctre::phoenix::CANifier::LEDChannelA);
+          last_green = status_light->green;
+        }
+
+        if (status_light->blue != last_blue) {
+          canifier_.SetLEDOutput(1.0 - status_light->blue,
+                                 ::ctre::phoenix::CANifier::LEDChannelB);
+          last_blue = status_light->blue;
+        }
+
+        if (status_light->red != last_red) {
+          canifier_.SetLEDOutput(1.0 - status_light->red,
+                                 ::ctre::phoenix::CANifier::LEDChannelC);
+          last_red = status_light->red;
+        }
       }
     }
   }