commit 813b9afb7a86ec4539a24f7871fa49bf215aa4f9
author Austin Schuh <austin@peloton-tech.com> Sun Mar 08 18:46:58 2015 -0700
committer Austin Schuh <austin@peloton-tech.com> Sun Mar 08 19:31:43 2015 -0700
tree 151b65b133c1bf815369d0ee88b9a87b26fec155
parent 95de062b75339fc39ed8182e0c446d96b03edf48

Added a bunch of new actions.

Change-Id: I890b4cef168baa12e02e181de683243ffa02e7c0

frc971/actors/stack_actor.cc

diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index 7267a4b..6f350d3 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -24,29 +24,41 @@
 
 bool StackActor::RunAction(const StackParams &params) {
   const auto &values = constants::GetValues();
-  const double bottom = 0.020;
 
   // Set the current stack down on top of the bottom box.
-  DoFridgeProfile(0.39, 0.0, kSlowArmMove, kReallySlowElevatorMove, true);
+  DoFridgeProfile(params.over_box_before_place_height, 0.0, kSlowArmMove,
+                  kReallySlowElevatorMove, true);
   if (ShouldCancel()) return true;
   // Set down on the box.
-  DoFridgeProfile(bottom + values.tote_height, 0.0, kSlowArmMove,
+  DoFridgeProfile(params.bottom + values.tote_height, 0.0, kSlowArmMove,
                   kSlowElevatorMove, true);
   if (ShouldCancel()) return true;
   // Clamp.
   {
-    auto message = control_loops::claw_queue.goal.MakeMessage();
-    message->angle = params.claw_out_angle;
-    message->angular_velocity = 0.0;
-    message->intake = 0.0;
-    message->rollers_closed = true;
+    bool send_goal = true;
+    control_loops::claw_queue.status.FetchLatest();
+    if (control_loops::claw_queue.status.get()) {
+      if (control_loops::claw_queue.status->goal_angle <
+          params.claw_out_angle) {
+        send_goal = false;
+      }
+    }
+    if (send_goal) {
+      auto message = control_loops::claw_queue.goal.MakeMessage();
+      message->angle = params.claw_out_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();
+      LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+      message.Send();
+    }
   }
-  DoFridgeProfile(bottom, -0.05, kFastArmMove, kFastElevatorMove, false);
+  DoFridgeProfile(params.bottom, -0.05, kFastArmMove, kFastElevatorMove, false);
   if (ShouldCancel()) return true;
-  DoFridgeProfile(bottom, 0.0, kFastArmMove, kFastElevatorMove, false);
+  DoFridgeProfile(params.bottom, 0.0, kFastArmMove, kFastElevatorMove, false);
   if (ShouldCancel()) return true;
   aos::time::SleepFor(aos::time::Time::InMS(100));