commit 697d07bb629e211f543f935fd11f0987fc6fe780
author Brian Silverman <brians> Thu Mar 19 23:41:11 2015 -0700
committer Brian Silverman <brians> Fri Mar 27 22:53:20 2015 -0400
tree 929f3836993dbc8ff5cf3f50ca09d73db231cb4e
parent cb615cff50b15b45ba415207e6d3275227dc1fae

make stacking faster

Change-Id: I78e5159cbf014551c6e561c702070478a01c4dc8

frc971/actors/lift_action_params.q

diff --git a/frc971/actors/lift_action_params.q b/frc971/actors/lift_action_params.q
index a5a1403..7aa29b8 100644
--- a/frc971/actors/lift_action_params.q
+++ b/frc971/actors/lift_action_params.q
@@ -6,4 +6,9 @@
   double lift_height;
   // Arm goal.
   double lift_arm;
+
+  // True to move the claw in the middle of the lift.
+  bool pack_claw;
+  // Iff pack_claw is true, the angle to move the claw to.
+  double pack_claw_angle;
 };

frc971/actors/lift_actor.cc

diff --git a/frc971/actors/lift_actor.cc b/frc971/actors/lift_actor.cc
index 60ef631..44222fd 100644
--- a/frc971/actors/lift_actor.cc
+++ b/frc971/actors/lift_actor.cc
@@ -4,6 +4,7 @@
 #include "frc971/actors/lift_actor.h"
 #include "frc971/constants.h"
 #include "frc971/actors/fridge_profile_lib.h"
+#include "frc971/control_loops/claw/claw.q.h"
 
 namespace frc971 {
 namespace actors {
@@ -45,6 +46,18 @@
         }
         goal_angle = params.lift_arm;
         has_started_back = true;
+        if (params.pack_claw) {
+          auto message = control_loops::claw_queue.goal.MakeMessage();
+          message->angle = params.pack_claw_angle;
+          message->angular_velocity = 0.0;
+          message->intake = 0.0;
+          message->rollers_closed = true;
+          message->max_velocity = 6.0;
+          message->max_acceleration = 10.0;
+
+          LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+          message.Send();
+        }
       }
     }
 

frc971/actors/stack_and_lift_actor.cc

diff --git a/frc971/actors/stack_and_lift_actor.cc b/frc971/actors/stack_and_lift_actor.cc
index ad2263f..273f352 100644
--- a/frc971/actors/stack_and_lift_actor.cc
+++ b/frc971/actors/stack_and_lift_actor.cc
@@ -18,6 +18,17 @@
     : FridgeActorBase<StackAndLiftActionQueueGroup>(queues) {}
 
 bool StackAndLiftActor::RunAction(const StackAndLiftParams &params) {
+  control_loops::claw_queue.goal.FetchLatest();
+  double claw_goal_start;
+  bool have_claw_goal_start;
+  if (control_loops::claw_queue.goal.get()) {
+    claw_goal_start = control_loops::claw_queue.goal->angle;
+    have_claw_goal_start = true;
+  } else {
+    claw_goal_start = 0;
+    have_claw_goal_start = false;
+  }
+
   {
     StackParams stack_params = params.stack_params;
     stack_params.only_place = false;
@@ -58,8 +69,10 @@
   }
 
   {
-    ::std::unique_ptr<LiftAction> lift_action =
-        MakeLiftAction(params.lift_params);
+    auto lift_params = params.lift_params;
+    lift_params.pack_claw = have_claw_goal_start;
+    lift_params.pack_claw_angle = claw_goal_start;
+    ::std::unique_ptr<LiftAction> lift_action = MakeLiftAction(lift_params);
     lift_action->Start();
     while (lift_action->Running()) {
       ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),

frc971/joystick_reader.cc

diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 4ff9684..0d47606 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -109,6 +109,9 @@
 const double kClawTotePackAngle = 0.95;
 const double kArmRaiseLowerClearance = -0.08;
 
+const double kStackUpHeight = 0.55;
+const double kStackUpArm = 0.1;
+
 class Reader : public ::aos::input::JoystickInput {
  public:
   Reader() : was_running_(false) {}
@@ -265,8 +268,8 @@
 
       params.grab_after_stack = true;
       params.clamp_pause_time = 0.1;
-      params.lift_params.lift_height = 0.45;
-      params.lift_params.lift_arm = 0.3;
+      params.lift_params.lift_height = kStackUpHeight;
+      params.lift_params.lift_arm = kStackUpArm;
       params.grab_after_lift = true;
       fridge_closed_ = true;
 
@@ -308,8 +311,8 @@
       params.before_lift_settle_time = 0.1;
       params.bottom_height = 0.020;
       params.claw_out_angle = 0.6;
-      params.lift_params.lift_height = 0.45;
-      params.lift_params.lift_arm = 0.3;
+      params.lift_params.lift_height = kStackUpHeight;
+      params.lift_params.lift_arm = kStackUpArm;
       fridge_closed_ = true;
 
       action_queue_.EnqueueAction(actors::MakeHeldToLiftAction(params));
@@ -320,6 +323,8 @@
       actors::LiftParams params;
       params.lift_height = 0.68;
       params.lift_arm = 0.3;
+      params.pack_claw = false;
+      params.pack_claw_angle = 0;
       fridge_closed_ = true;
 
       action_queue_.EnqueueAction(actors::MakeLiftAction(params));