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/wpilib_interface.cc

diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 0fdc616..65b2246 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -473,6 +473,11 @@
     lights_ = ::std::move(s);
   }
 
+  void set_rgb_light(
+      ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+    rgb_lights_ = ::std::move(s);
+  }
+
   void operator()() {
     ::aos::SetCurrentThreadName("Solenoids");
     ::aos::SetCurrentThreadRealtimePriority(27);
@@ -493,6 +498,9 @@
         if (superstructure_.get()) {
           LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
           lights_->Set(superstructure_->lights_on);
+          rgb_lights_->Set(superstructure_->red_light_on |
+                           superstructure_->green_light_on |
+                           superstructure_->blue_light_on);
         }
       }
 
@@ -505,7 +513,7 @@
  private:
   ::frc971::wpilib::BufferedPcm pcm_;
 
-  ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> lights_;
+  ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> lights_, rgb_lights_;
 
   ::aos::Queue<
       ::y2017::control_loops::SuperstructureQueue::Output>
@@ -572,6 +580,16 @@
     hood_victor_ = ::std::move(t);
   }
 
+  void set_red_light(::std::unique_ptr<DigitalOutput> t) {
+    red_light_ = ::std::move(t);
+  }
+  void set_green_light(::std::unique_ptr<DigitalOutput> t) {
+    green_light_ = ::std::move(t);
+  }
+  void set_blue_light(::std::unique_ptr<DigitalOutput> t) {
+    blue_light_ = ::std::move(t);
+  }
+
  private:
   virtual void Read() override {
     ::y2017::control_loops::superstructure_queue.output.FetchAnother();
@@ -593,6 +611,10 @@
         ::aos::Clip(queue->voltage_hood, -kMaxBringupPower, kMaxBringupPower) /
         12.0);
     shooter_victor_->SetSpeed(queue->voltage_shooter / 12.0);
+
+    red_light_->Set(queue->red_light_on);
+    green_light_->Set(queue->green_light_on);
+    blue_light_->Set(queue->blue_light_on);
   }
 
   virtual void Stop() override {
@@ -604,11 +626,17 @@
     turret_victor_->SetDisabled();
     hood_victor_->SetDisabled();
     shooter_victor_->SetDisabled();
+
+    red_light_->Set(true);
+    green_light_->Set(true);
+    blue_light_->Set(true);
   }
 
   ::std::unique_ptr<VictorSP> intake_victor_, intake_rollers_victor_,
       indexer_victor_, indexer_roller_victor_, shooter_victor_,
       turret_victor_, hood_victor_;
+
+  ::std::unique_ptr<DigitalOutput> red_light_, green_light_, blue_light_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -685,11 +713,20 @@
         ::std::unique_ptr<VictorSP>(new VictorSP(2)));
     superstructure_writer.set_shooter_victor(
         ::std::unique_ptr<VictorSP>(new VictorSP(8)));
+
+    superstructure_writer.set_red_light(
+        ::std::unique_ptr<DigitalOutput>(new DigitalOutput(5)));
+    superstructure_writer.set_green_light(
+        ::std::unique_ptr<DigitalOutput>(new DigitalOutput(24)));
+    superstructure_writer.set_blue_light(
+        ::std::unique_ptr<DigitalOutput>(new DigitalOutput(25)));
+
     ::std::thread superstructure_writer_thread(
         ::std::ref(superstructure_writer));
 
     SolenoidWriter solenoid_writer;
     solenoid_writer.set_lights(solenoid_writer.pcm()->MakeSolenoid(0));
+    solenoid_writer.set_rgb_light(solenoid_writer.pcm()->MakeSolenoid(1));
 
     ::std::thread solenoid_thread(::std::ref(solenoid_writer));