commit 71466fee065243bcad915ccc5f7fcb214030fc45
author Comran Morshed <comranmorsh@gmail.com> Thu Apr 21 20:21:14 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Apr 24 19:15:36 2016 -0700
tree 1d2b1cad0012afcc7354eb54be5234ce0b38224b
parent e8b1dfe5ca043d4367c1d28a6d5bd48d5c98c61b

Add climber framework.

Change-Id: I807de18bb204f4da921332b2074e4d343d3fd8dd

y2016/wpilib/wpilib_interface.cc

diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index b3ed0a8..acf820f 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -428,14 +428,14 @@
     compressor_ = ::std::move(compressor);
   }
 
-  void set_drivetrain_left(
+  void set_drivetrain_shifter(
       ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
-    drivetrain_left_ = ::std::move(s);
+    drivetrain_shifter_ = ::std::move(s);
   }
 
-  void set_drivetrain_right(
+  void set_climber_trigger(
       ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
-    drivetrain_right_ = ::std::move(s);
+    climber_trigger_ = ::std::move(s);
   }
 
   void set_traverse(
@@ -487,8 +487,8 @@
         drivetrain_.FetchLatest();
         if (drivetrain_.get()) {
           LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
-          drivetrain_left_->Set(!drivetrain_->left_high);
-          drivetrain_right_->Set(!drivetrain_->right_high);
+          drivetrain_shifter_->Set(
+              !(drivetrain_->left_high || drivetrain_->right_high));
         }
       }
 
@@ -518,7 +518,10 @@
       {
         superstructure_.FetchLatest();
         if (superstructure_.get()) {
-          LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+          LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+
+          climber_trigger_->Set(superstructure_->unfold_climber);
+
           traverse_->Set(superstructure_->traverse_down);
           traverse_latch_->Set(superstructure_->traverse_unlatched);
         }
@@ -542,9 +545,9 @@
  private:
   const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
 
-  ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
-      drivetrain_right_, shooter_clamp_, shooter_pusher_, lights_, traverse_,
-      traverse_latch_;
+  ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_,
+      shooter_clamp_, shooter_pusher_, lights_, traverse_, traverse_latch_,
+      climber_trigger_;
   ::std::unique_ptr<Relay> flashlight_;
   ::std::unique_ptr<Compressor> compressor_;
 
@@ -642,6 +645,10 @@
     bottom_rollers_talon_ = ::std::move(t);
   }
 
+  void set_climber_talon(::std::unique_ptr<Talon> t) {
+    climber_talon_ = ::std::move(t);
+  }
+
  private:
   virtual void Read() override {
     ::y2016::control_loops::superstructure_queue.output.FetchAnother();
@@ -661,6 +668,7 @@
         12.0);
     top_rollers_talon_->Set(-queue->voltage_top_rollers / 12.0);
     bottom_rollers_talon_->Set(-queue->voltage_bottom_rollers / 12.0);
+    climber_talon_->Set(-queue->voltage_climber / 12.0);
   }
 
   virtual void Stop() override {
@@ -671,7 +679,7 @@
   }
 
   ::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_,
-      top_rollers_talon_, bottom_rollers_talon_;
+      top_rollers_talon_, bottom_rollers_talon_, climber_talon_;
 };
 
 class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -754,19 +762,21 @@
         ::std::unique_ptr<Talon>(new Talon(1)));
     superstructure_writer.set_bottom_rollers_talon(
         ::std::unique_ptr<Talon>(new Talon(0)));
+    superstructure_writer.set_climber_talon(
+        ::std::unique_ptr<Talon>(new Talon(7)));
     ::std::thread superstructure_writer_thread(
         ::std::ref(superstructure_writer));
 
     ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
         new ::frc971::wpilib::BufferedPcm());
     SolenoidWriter solenoid_writer(pcm);
-    solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(1));
-    solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(0));
+    solenoid_writer.set_drivetrain_shifter(pcm->MakeSolenoid(0));
     solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
     solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
     solenoid_writer.set_shooter_clamp(pcm->MakeSolenoid(4));
     solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(5));
     solenoid_writer.set_lights(pcm->MakeSolenoid(6));
+    solenoid_writer.set_climber_trigger(pcm->MakeSolenoid(1));
     solenoid_writer.set_flashlight(make_unique<Relay>(0));
 
     solenoid_writer.set_compressor(make_unique<Compressor>());