commit 6e242ac95b6f78c9fb772d2ddeb62cdc772814a3
author Austin Schuh <austin@peloton-tech.com> Sat Mar 07 17:08:21 2015 -0800
committer Brian Silverman <brians> Sat Mar 07 21:25:11 2015 -0500
tree 8c21ff032cd09745f9b797c5f5b0de63e541e21f
parent e52efc2b077ba5070b69a30ff25b9cd74b927ab7

Removed all uses of the fridge profile actor.

Change-Id: Ic59aec97efcd91f268e8d5d3bc5fb112d77a6199

frc971/actors/stack_actor.cc

diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index ac2d744..7267a4b 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -1,64 +1,37 @@
+#include "frc971/actors/stack_actor.h"
+
 #include <math.h>
 
 #include "aos/common/time.h"
 #include "aos/common/util/phased_loop.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 {
+constexpr ProfileParams kSlowArmMove{0.8, 1.4};
+constexpr ProfileParams kSlowElevatorMove{0.5, 3.0};
+constexpr ProfileParams kReallySlowElevatorMove{0.10, 1.0};
 
-static constexpr double kArmVelocity = 0.40;
-static constexpr double kArmAcceleration = 1.0;
-static constexpr double kElevatorVelocity = 0.5;
-static constexpr double kElevatorAcceleration = 2.2;
-
+constexpr ProfileParams kFastArmMove{0.8, 4.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
 }  // namespace
 
 StackActor::StackActor(StackActionQueueGroup *queues)
-    : aos::common::actions::ActorBase<StackActionQueueGroup>(queues) {}
-
-void StackActor::DoProfile(double height, double angle, bool grabbers) {
-  FridgeProfileParams params;
-
-  params.elevator_height = height;
-  params.elevator_max_velocity = kElevatorVelocity;
-  params.elevator_max_acceleration = kElevatorAcceleration;
-
-  params.arm_angle = angle;
-  params.arm_max_velocity = kArmVelocity;
-  params.arm_max_acceleration = kArmAcceleration;
-
-  params.top_front_grabber = grabbers;
-  params.top_back_grabber = grabbers;
-  params.bottom_front_grabber = grabbers;
-  params.bottom_back_grabber = grabbers;
-
-  ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
-  profile->Start();
-  while (!profile->CheckIteration()) {
-    // wait until next Xms tick
-    ::aos::time::PhasedLoopXMS(5, 2500);
-    if (ShouldCancel()) {
-      profile->Cancel();
-      return;
-    }
-  }
-}
+    : FridgeActorBase<StackActionQueueGroup>(queues) {}
 
 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(0.45, 0.0, true);
+  DoFridgeProfile(0.39, 0.0, kSlowArmMove, kReallySlowElevatorMove, true);
   if (ShouldCancel()) return true;
-  // Move down to enclose bottom box.
-  DoProfile(bottom + values.tote_height, 0.0, true);
+  // Set down on the box.
+  DoFridgeProfile(bottom + values.tote_height, 0.0, kSlowArmMove,
+                  kSlowElevatorMove, true);
   if (ShouldCancel()) return true;
   // Clamp.
   {
@@ -71,9 +44,9 @@
     LOG_STRUCT(DEBUG, "Sending claw goal", *message);
     message.Send();
   }
-  DoProfile(bottom, -0.05, false);
+  DoFridgeProfile(bottom, -0.05, kFastArmMove, kFastElevatorMove, false);
   if (ShouldCancel()) return true;
-  DoProfile(bottom, 0.0, false);
+  DoFridgeProfile(bottom, 0.0, kFastArmMove, kFastElevatorMove, false);
   if (ShouldCancel()) return true;
   aos::time::SleepFor(aos::time::Time::InMS(100));