commit 3cba37970437f99f18de9289bdfa3536918de81b
author Austin Schuh <austin@peloton-tech.com> Sun Apr 19 20:01:22 2015 -0700
committer Brian Silverman <brians> Thu May 14 00:41:07 2015 -0400
tree 7de19129b8b65f5bb9c885f5311be6eebeea6132
parent 4db921a10c913f8f6a0f854ba07190c3c1eea7e6

Added pause while lifing the can.

Change-Id: I8ff43073566592fc0d8de7ddd693842adf86be2d

frc971/actors/lift_action_params.q

diff --git a/frc971/actors/lift_action_params.q b/frc971/actors/lift_action_params.q
index 7aa29b8..b31982b 100644
--- a/frc971/actors/lift_action_params.q
+++ b/frc971/actors/lift_action_params.q
@@ -7,6 +7,11 @@
   // Arm goal.
   double lift_arm;
 
+  // If true, do the second lift.
+  bool second_lift;
+  // Arm goal.
+  double intermediate_lift_height;
+
   // 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.

frc971/actors/lift_actor.cc

diff --git a/frc971/actors/lift_actor.cc b/frc971/actors/lift_actor.cc
index 44222fd..ff36bef 100644
--- a/frc971/actors/lift_actor.cc
+++ b/frc971/actors/lift_actor.cc
@@ -11,6 +11,7 @@
 namespace {
 constexpr ProfileParams kArmMove{0.6, 1.0};
 constexpr ProfileParams kElevatorMove{0.9, 3.0};
+constexpr ProfileParams kElevatorFixMove{0.9, 2.0};
 }  // namespace
 
 LiftActor::LiftActor(LiftActionQueueGroup *queues)
@@ -25,6 +26,15 @@
   double goal_height = params.lift_height;
   double goal_angle = 0.0;
 
+  if (params.second_lift) {
+    DoFridgeProfile(params.intermediate_lift_height, 0.0, kElevatorFixMove,
+                    kArmMove,
+                    control_loops::fridge_queue.status->grabbers.top_front,
+                    control_loops::fridge_queue.status->grabbers.bottom_front,
+                    control_loops::fridge_queue.status->grabbers.bottom_back);
+    if (ShouldCancel()) return true;
+  }
+
   if (!StartFridgeProfile(
           params.lift_height, 0.0, kElevatorMove, kArmMove,
           control_loops::fridge_queue.status->grabbers.top_front,

frc971/autonomous/auto.cc

diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 42f4ca9..399416c 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -400,6 +400,7 @@
     params.claw_out_angle = kClawStackClearance;
     params.lift_params.lift_height = kStackUpHeight;
     params.lift_params.lift_arm = kStackUpArm;
+    params.lift_params.second_lift = false;
 
     lift = actors::MakeHeldToLiftAction(params);
     lift->Start();

frc971/joystick_reader.cc

diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index e85240f..7dd7024 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -290,6 +290,7 @@
       params.clamp_pause_time = 0.0;
       params.lift_params.lift_height = kStackUpHeight;
       params.lift_params.lift_arm = kStackUpArm;
+      params.lift_params.second_lift = false;
       params.grab_after_lift = true;
       fridge_closed_ = true;
 
@@ -333,6 +334,7 @@
       params.claw_out_angle = kClawStackClearance;
       params.lift_params.lift_height = kStackUpHeight;
       params.lift_params.lift_arm = kStackUpArm;
+      params.lift_params.second_lift = false;
       fridge_closed_ = true;
 
       action_queue_.EnqueueAction(actors::MakeHeldToLiftAction(params));
@@ -345,6 +347,8 @@
       params.lift_arm = 0.3;
       params.pack_claw = false;
       params.pack_claw_angle = 0;
+      params.intermediate_lift_height = 0.37;
+      params.second_lift = true;
       fridge_closed_ = true;
 
       action_queue_.EnqueueAction(actors::MakeLiftAction(params));