commit dc1710163a86bd3746090fca3ae37ada843a4a3e
author Austin Schuh <austin@peloton-tech.com> Sun Feb 22 21:36:55 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Sun Feb 22 21:36:55 2015 -0800
tree 425b3543aec87182e6e6126ffa4abacdd1e1ba6d
parent 867c11e3b44b38191414061761510af29fc3c5fd

Made stack action a lot closer to working.

Change-Id: Ieb0e0c1acfd7082667e3eed7314a53bf923f34ee

frc971/actors/actors.gyp

diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 5daed38..1b89982 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -194,6 +194,7 @@
         '<(AOS)/build/aos.gyp:logging',
         '<(AOS)/common/actions/actions.gyp:action_lib',
         '<(DEPTH)/frc971/frc971.gyp:constants',
+        '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
       ],
       'export_dependent_settings': [
         '<(AOS)/common/actions/actions.gyp:action_lib',

frc971/actors/stack_action.q

diff --git a/frc971/actors/stack_action.q b/frc971/actors/stack_action.q
index 6ef4fd8..caa8abf 100644
--- a/frc971/actors/stack_action.q
+++ b/frc971/actors/stack_action.q
@@ -2,12 +2,18 @@
 
 import "aos/common/actions/actions.q";
 
+// Parameters to send with start.
+struct StackParams {
+  // Angle to move the claw to when picking up.
+  double claw_out_angle;
+};
+
 queue_group StackActionQueueGroup {
   implements aos.common.actions.ActionQueueGroup;
 
   message Goal {
     uint32_t run;
-    uint32_t params;
+    StackParams params;
   };
 
   queue Goal goal;

frc971/actors/stack_actor.cc

diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index 6ce8fbd..bf5a3e3 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -1,36 +1,37 @@
 #include <math.h>
 
+#include "aos/common/time.h"
 #include "frc971/actors/stack_actor.h"
 #include "frc971/actors/fridge_profile_actor.h"
 #include "frc971/constants.h"
+#include "frc971/control_loops/claw/claw.q.h"
 
 namespace frc971 {
 namespace actors {
 namespace {
 
-// TODO(danielp): Real numbers!
-constexpr double kElevatorMaxVelocity = 0.5;
-constexpr double kArmMaxVelocity = 0.5;
-constexpr double kElevatorMaxAccel = 0.25;
-constexpr double kArmMaxAccel = 0.25;
+static constexpr double kArmVelocity = 0.40;
+static constexpr double kArmAcceleration = 1.0;
+static constexpr double kElevatorVelocity = 0.5;
+static constexpr double kElevatorAcceleration = 2.2;
 
 }  // namespace
 
-StackActor::StackActor(StackActionQueueGroup* queues)
+StackActor::StackActor(StackActionQueueGroup *queues)
     : aos::common::actions::ActorBase<StackActionQueueGroup>(queues) {}
 
 namespace {
 
-void DoProfile(double height, bool grabbers) {
+void DoProfile(double height, double angle, bool grabbers) {
   FridgeProfileParams params;
 
   params.elevator_height = height;
-  params.elevator_max_velocity = kElevatorMaxVelocity;
-  params.elevator_max_acceleration = kElevatorMaxAccel;
+  params.elevator_max_velocity = kElevatorVelocity;
+  params.elevator_max_acceleration = kElevatorAcceleration;
 
-  params.arm_angle = 0.0;
-  params.arm_max_velocity = kArmMaxVelocity;
-  params.arm_max_acceleration = kArmMaxAccel;
+  params.arm_angle = angle;
+  params.arm_max_velocity = kArmVelocity;
+  params.arm_max_acceleration = kArmAcceleration;
 
   params.top_front_grabber = grabbers;
   params.top_back_grabber = grabbers;
@@ -44,23 +45,35 @@
 
 }  // namespace
 
-bool StackActor::RunAction(const uint32_t&) {
-  const auto& values = constants::GetValues();
-  const double bottom = values.fridge.elevator.lower_limit;
+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.
-  DoProfile(bottom + values.tote_height, true);
+  DoProfile(0.45, 0.0, true);
   // Move down to enclose bottom box.
-  DoProfile(bottom, false);
+  DoProfile(bottom + values.tote_height, 0.0, true);
   // Clamp.
-  DoProfile(bottom, true);
+  {
+    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;
+
+    LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+    message.Send();
+  }
+  DoProfile(bottom, -0.05, false);
+  DoProfile(bottom, 0.0, false);
+  aos::time::SleepFor(aos::time::Time::InMS(100));
 
   return true;
 }
 
-::std::unique_ptr<StackAction> MakeStackAction() {
+::std::unique_ptr<StackAction> MakeStackAction(const StackParams &params) {
   return ::std::unique_ptr<StackAction>(
-      new StackAction(&::frc971::actors::stack_action, 0));
+      new StackAction(&::frc971::actors::stack_action, params));
 }
 
 }  // namespace actors

frc971/actors/stack_actor.h

diff --git a/frc971/actors/stack_actor.h b/frc971/actors/stack_actor.h
index 2c37770..8e834e7 100644
--- a/frc971/actors/stack_actor.h
+++ b/frc971/actors/stack_actor.h
@@ -17,13 +17,13 @@
  public:
   explicit StackActor(StackActionQueueGroup *queues);
 
-  bool RunAction(const uint32_t&) override;
+  bool RunAction(const StackParams &params) override;
 };
 
 typedef aos::common::actions::TypedAction<StackActionQueueGroup> StackAction;
 
 // Makes a new stackActor action.
-::std::unique_ptr<StackAction> MakeStackAction();
+::std::unique_ptr<StackAction> MakeStackAction(const StackParams &params);
 
 }  // namespace actors
 }  // namespace frc971