commit 95eb162b030efc7ee4ace96c37c154d446c3d0af
author Austin Schuh <austin@peloton-tech.com> Sat Mar 14 21:11:22 2015 -0700
committer Austin Schuh <austin@peloton-tech.com> Sat Mar 14 23:08:05 2015 -0700
tree 5576052e5b515c924265ead383f83b711d2470b8
parent fa65bfde21ac1149f9bf11103e5b1d8288677448

Rounded lift actor corner.

Change-Id: I3f3ac54cd444f873aa00c843798711e4bdade430

frc971/actors/lift_actor.cc

diff --git a/frc971/actors/lift_actor.cc b/frc971/actors/lift_actor.cc
index 9809cc6..8289643 100644
--- a/frc971/actors/lift_actor.cc
+++ b/frc971/actors/lift_actor.cc
@@ -8,7 +8,7 @@
 namespace frc971 {
 namespace actors {
 namespace {
-constexpr ProfileParams kArmMove{0.6, 2.0};
+constexpr ProfileParams kArmMove{0.6, 1.0};
 constexpr ProfileParams kElevatorMove{0.9, 3.0};
 }  // namespace
 
@@ -21,19 +21,43 @@
     return false;
   }
 
-  // Lift the box straight up.
-  DoFridgeProfile(params.lift_height, 0.0, kElevatorMove, 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;
+  double goal_height = params.lift_height;
+  double goal_angle = 0.0;
 
-  // Move it back to the storage location.
-  DoFridgeProfile(params.lift_height, params.lift_arm, kElevatorMove, 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 (!StartFridgeProfile(
+          params.lift_height, 0.0, kElevatorMove, 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)) {
+    return true;
+  }
+
+  bool has_started_back = false;
+  while (true) {
+    if (control_loops::fridge_queue.status->goal_height > 0.1) {
+      if (!has_started_back) {
+        if (!StartFridgeProfile(
+                params.lift_height, params.lift_arm, kElevatorMove, 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)) {
+          return true;
+        }
+        goal_angle = params.lift_arm;
+        has_started_back = true;
+      }
+    }
+
+    ProfileStatus status = IterateProfile(
+        goal_height, goal_angle, kElevatorMove, 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 (status == DONE || status == CANCELED) {
+      return true;
+    }
+  }
   return true;
 }