commit b79c424c04a4eb145d562b11bed6bddb3896dfa2
author Comran Morshed <comranmorsh@gmail.com> Sat Feb 06 18:27:26 2016 +0000
committer Comran Morshed <comranmorsh@gmail.com> Sun Feb 14 17:14:11 2016 +0000
tree 28554255243780e7879b837318b84ea579cfabc1
parent 225f0b93e9094c4c8fc0c0e1e71841c4e785cf60

Add shooter pneumatics.

Change-Id: I2568af8b2836b17d950c24da00c830f0c774f1b7

y2016/wpilib/wpilib_interface.cc

diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 156f90a..8ec212f 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -32,6 +32,7 @@
 
 #include "frc971/control_loops/control_loops.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
 #include "y2016/constants.h"
 #include "y2016/control_loops/shooter/shooter.q.h"
 #include "y2016/control_loops/superstructure/superstructure.q.h"
@@ -346,7 +347,8 @@
   ::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
 
   ::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
-  ::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_, shoulder_encoder_, wrist_encoder_;
+  ::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_,
+      shoulder_encoder_, wrist_encoder_;
 
   ::std::atomic<bool> run_{true};
   DigitalGlitchFilter encoder_filter_, hall_filter_;
@@ -356,7 +358,8 @@
  public:
   SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
       : pcm_(pcm),
-        drivetrain_(".frc971.control_loops.drivetrain_queue.output") {}
+        drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
+        shooter_(".y2016.control_loops.shooter_queue.output") {}
 
   void set_pressure_switch(::std::unique_ptr<DigitalInput> pressure_switch) {
     pressure_switch_ = ::std::move(pressure_switch);
@@ -376,6 +379,16 @@
     drivetrain_right_ = ::std::move(s);
   }
 
+  void set_shooter_clamp(
+      ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+    shooter_clamp_ = ::std::move(s);
+  }
+
+  void set_shooter_pusher(
+      ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+    shooter_pusher_ = ::std::move(s);
+  }
+
   void operator()() {
     ::aos::SetCurrentThreadName("Solenoids");
     ::aos::SetCurrentThreadRealtimePriority(27);
@@ -401,6 +414,15 @@
       }
 
       {
+        shooter_.FetchLatest();
+        if (shooter_.get()) {
+          LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+          shooter_clamp_->Set(shooter_->clamp_open);
+          shooter_pusher_->Set(shooter_->push_to_shooter);
+        }
+      }
+
+      {
         ::frc971::wpilib::PneumaticsToLog to_log;
         {
           const bool compressor_on = !pressure_switch_->Get();
@@ -425,12 +447,12 @@
   const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
 
   ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
-      drivetrain_right_;
-
+      drivetrain_right_, shooter_clamp_, shooter_pusher_;
   ::std::unique_ptr<DigitalInput> pressure_switch_;
   ::std::unique_ptr<Relay> compressor_relay_;
 
   ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+  ::aos::Queue<::y2016::control_loops::ShooterQueue::Output> shooter_;
 
   ::std::atomic<bool> run_{true};
 };
@@ -608,6 +630,8 @@
     SolenoidWriter solenoid_writer(pcm);
     solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
     solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(7));
+    solenoid_writer.set_shooter_clamp(pcm->MakeSolenoid(6));
+    solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(7));
 
     solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(25));
     solenoid_writer.set_compressor_relay(make_unique<Relay>(0));