commit 93936f7246279c9b015f995ffafa8c68e5f15620
author Brian Silverman <brians> Thu Mar 19 23:38:30 2015 -0700
committer Brian Silverman <brians> Fri Mar 27 22:53:20 2015 -0400
tree 1a18062a26b50e810566e8457ab46189a1ab5975
parent 5bfda6c2d636406e614e749a819eba4e77be6742

implement an autonomous which grabs two cans

Change-Id: I7b0d262b986a8f0397df90f5702b3f4d8d1334b9

frc971/autonomous/auto.cc

diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e420aa6..9f0a033 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -20,6 +20,7 @@
 using ::aos::time::Time;
 using ::frc971::control_loops::claw_queue;
 using ::frc971::control_loops::fridge_queue;
+using ::frc971::control_loops::drivetrain_queue;
 
 namespace frc971 {
 namespace autonomous {
@@ -193,6 +194,28 @@
   if (ShouldExitAuto()) return;
   InitializeEncoders();
 
+  time::SleepFor(time::Time::InSeconds(0.55));
+
+  if (!drivetrain_queue.goal.MakeWithBuilder()
+           .steering(0)
+           .throttle(0.5)
+           .quickturn(false)
+           .control_loop_driving(false)
+           .Send()) {
+    LOG(WARNING, "sending drivetrain goal failed\n");
+  }
+  time::SleepFor(time::Time::InSeconds(1.0));
+  if (!drivetrain_queue.goal.MakeWithBuilder()
+           .steering(0)
+           .throttle(0)
+           .quickturn(false)
+           .control_loop_driving(false)
+           .Send()) {
+    LOG(WARNING, "sending drivetrain goal failed\n");
+  }
+
+  return;
+
   {
     auto new_fridge_goal = fridge_queue.goal.MakeMessage();
     new_fridge_goal->max_velocity = 0.0;

frc971/prime/start_list.txt

diff --git a/frc971/prime/start_list.txt b/frc971/prime/start_list.txt
index 6c2fa75..8a264ed 100644
--- a/frc971/prime/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -13,3 +13,4 @@
 stack_and_hold_action
 held_to_lift_action
 drivetrain
+auto

frc971/wpilib/wpilib_interface.cc

diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 8f41300..25da16a 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -398,6 +398,14 @@
     claw_pinchers_ = ::std::move(s);
   }
 
+  void set_grabber_latch_release(::std::unique_ptr<BufferedSolenoid> s) {
+    grabber_latch_release_ = ::std::move(s);
+  }
+
+  void set_grabber_fold_up(::std::unique_ptr<BufferedSolenoid> s) {
+    grabber_fold_up_ = ::std::move(s);
+  }
+
   void operator()() {
     ::aos::SetCurrentThreadName("Solenoids");
     ::aos::SetCurrentThreadRealtimePriority(30);
@@ -424,6 +432,12 @@
         }
       }
 
+      ::aos::joystick_state.FetchLatest();
+      grabber_latch_release_->Set(::aos::joystick_state.get() != nullptr &&
+                                  ::aos::joystick_state->autonomous);
+      grabber_fold_up_->Set(::aos::joystick_state.get() != nullptr &&
+                            ::aos::joystick_state->joysticks[1].buttons & 1);
+
       {
         PneumaticsToLog to_log;
         {
@@ -452,6 +466,8 @@
   ::std::unique_ptr<BufferedSolenoid> fridge_grabbers_bottom_front_;
   ::std::unique_ptr<BufferedSolenoid> fridge_grabbers_bottom_back_;
   ::std::unique_ptr<BufferedSolenoid> claw_pinchers_;
+  ::std::unique_ptr<BufferedSolenoid> grabber_latch_release_;
+  ::std::unique_ptr<BufferedSolenoid> grabber_fold_up_;
   ::std::unique_ptr<DigitalSource> pressure_switch_;
   ::std::unique_ptr<Relay> compressor_relay_;
 
@@ -665,6 +681,8 @@
     solenoid_writer.set_fridge_grabbers_bottom_front(pcm->MakeSolenoid(2));
     solenoid_writer.set_fridge_grabbers_bottom_back(pcm->MakeSolenoid(1));
     solenoid_writer.set_claw_pinchers(pcm->MakeSolenoid(4));
+    solenoid_writer.set_grabber_latch_release(pcm->MakeSolenoid(7));
+    solenoid_writer.set_grabber_fold_up(pcm->MakeSolenoid(5));
 
     solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(9));
     solenoid_writer.set_compressor_relay(make_unique<Relay>(0));