commit bd01a5865570bbc65ed978ba207ea2766bffff21
author Austin Schuh <austin.linux@gmail.com> Mon Sep 21 00:05:31 2015 +0000
committer Austin Schuh <austin.linux@gmail.com> Mon Sep 28 00:37:08 2015 +0000
tree 349edf74ac90c6a57dc1174442b7aef33a8a324f
parent 34f891d53a95b1bafa346b28ffbbc6d91d47f911

Holds can in auto.

Change-Id: I3d1f9c473b58552cc6b216d88162a42256388ff8

bot3/wpilib/wpilib_interface.cc

diff --git a/bot3/wpilib/wpilib_interface.cc b/bot3/wpilib/wpilib_interface.cc
index 6926efe..36e14ad 100644
--- a/bot3/wpilib/wpilib_interface.cc
+++ b/bot3/wpilib/wpilib_interface.cc
@@ -201,10 +201,11 @@
 // Writes out our pneumatic outputs.
 class SolenoidWriter {
  public:
-  SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+  SolenoidWriter(const ::std::unique_ptr< ::frc971::wpilib::BufferedPcm> &pcm)
       : pcm_(pcm),
         elevator_(".bot3.control_loops.elevator_queue.output"),
-        intake_(".bot3.control_loops.intake_queue.output") {}
+        intake_(".bot3.control_loops.intake_queue.output"),
+        can_grabber_control_(".bot3.autonomous.can_grabber_control") {}
 
   void set_pressure_switch(::std::unique_ptr<DigitalSource> pressure_switch) {
     pressure_switch_ = ::std::move(pressure_switch);
@@ -219,6 +220,10 @@
     elevator_passive_support_ = ::std::move(elevator_passive_support);
   }
 
+  void set_can_grabber(::std::unique_ptr<BufferedSolenoid> can_grabber) {
+    can_grabber_ = ::std::move(can_grabber);
+  }
+
   void set_elevator_can_support(
       ::std::unique_ptr<BufferedSolenoid> elevator_can_support) {
     elevator_can_support_ = ::std::move(elevator_can_support);
@@ -235,6 +240,15 @@
     while (run_) {
       ::aos::time::PhasedLoopXMS(20, 1000);
 
+      // Can Grabber
+      {
+        can_grabber_control_.FetchLatest();
+        if (can_grabber_control_.get()) {
+          LOG_STRUCT(DEBUG, "solenoids", *can_grabber_control_);
+          can_grabber_->Set(can_grabber_control_->can_grabbers);
+        }
+      }
+
       // Elevator
       {
         elevator_.FetchLatest();
@@ -284,12 +298,14 @@
   ::std::unique_ptr<BufferedSolenoid> elevator_passive_support_;
   ::std::unique_ptr<BufferedSolenoid> elevator_can_support_;
   ::std::unique_ptr<BufferedSolenoid> intake_claw_;
+  ::std::unique_ptr<BufferedSolenoid> can_grabber_;
 
   ::std::unique_ptr<DigitalSource> pressure_switch_;
   ::std::unique_ptr<Relay> compressor_relay_;
 
   ::aos::Queue<::bot3::control_loops::ElevatorQueue::Output> elevator_;
   ::aos::Queue<::bot3::control_loops::IntakeQueue::Output> intake_;
+  ::aos::Queue<::bot3::autonomous::CanGrabberControl> can_grabber_control_;
 
   ::std::atomic<bool> run_{true};
 };
@@ -491,6 +507,7 @@
     solenoid_writer.set_elevator_passive_support(pcm->MakeSolenoid(0));
     solenoid_writer.set_elevator_can_support(pcm->MakeSolenoid(1));
     solenoid_writer.set_intake_claw(pcm->MakeSolenoid(2));
+    solenoid_writer.set_can_grabber(pcm->MakeSolenoid(3));
     ::std::thread solenoid_thread(::std::ref(solenoid_writer));
 
     // Wait forever. Not much else to do...