commit c587c88ee85cb08eee6fc7c923699fc97e90bdfc
author Austin Schuh <austin.linux@gmail.com> Wed Mar 29 21:33:10 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Apr 09 16:14:28 2017 -0700
tree 3ac256a7f8f63b1cc25bf6eeceee5c6ced9d94d7
parent 644e43dd83d710d20c3a20844a0672a3c68f73df

Added RGB lights.

Change-Id: Ibc4438acd7c678ebf2ed7c621875d9c6364637d1

y2017/control_loops/superstructure/superstructure.cc

diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 357c4b6..2a04ef4 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -96,6 +96,12 @@
                   output != nullptr ? &(output->voltage_turret) : nullptr,
                   &(status->indexer), &(status->turret), &intake_);
 
+  status->estopped =
+      status->intake.estopped | status->hood.estopped | status->turret.estopped;
+
+  status->zeroed =
+      status->intake.zeroed && status->hood.zeroed && status->turret.zeroed;
+
   if (output && unsafe_goal) {
     output->voltage_intake_rollers =
         ::std::max(-kMaxIntakeRollerVoltage,
@@ -108,6 +114,20 @@
 
     // Set the lights on or off
     output->lights_on = unsafe_goal->lights_on;
+
+    if (status->estopped) {
+      output->red_light_on = true;
+      output->green_light_on = false;
+      output->blue_light_on = false;
+    } else if (status->turret.vision_tracking) {
+      output->red_light_on = false;
+      output->green_light_on = true;
+      output->blue_light_on = false;
+    } else if (!status->zeroed) {
+      output->red_light_on = false;
+      output->green_light_on = false;
+      output->blue_light_on = true;
+    }
   }
 }