commit 457bde32705f442ad8425058abe794408fbc1cb2
author Austin Schuh <austin.linux@gmail.com> Sun Mar 17 18:16:41 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Fri Mar 22 20:03:41 2019 -0700
tree bc2b929c2dbaecf2ef0011456d43bdb7c2af2231
parent 139f59d3f64d550c173128ebc69b7ee03625993a

Make stilts come back down fast.

Change-Id: I9c32f14bde3831cba6fe866fb35d8236921f1805

y2019/joystick_reader.cc

diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 30d09a5..998ccd5 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -230,35 +230,6 @@
       switch_ball_ = true;
     }
 
-    // TODO(sabina): max height please?
-    if (data.IsPressed(kFallOver)) {
-      new_superstructure_goal->stilts.unsafe_goal = 0.71;
-      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
-      new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
-    } else if (data.IsPressed(kDeployStilt)) {
-      new_superstructure_goal->stilts.unsafe_goal = 0.50;
-      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
-      if (stilts_was_above_) {
-        new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
-      } else {
-        new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
-      }
-    } else if (data.IsPressed(kHalfStilt)) {
-      new_superstructure_goal->stilts.unsafe_goal = 0.345;
-      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
-      new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
-    } else {
-      new_superstructure_goal->stilts.unsafe_goal = 0.005;
-      new_superstructure_goal->stilts.profile_params.max_velocity = 0.25;
-      new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
-    }
-
-    if (superstructure_queue.status->stilts.position > 0.65) {
-      stilts_was_above_ = true;
-    } else if (superstructure_queue.status->stilts.position < 0.1) {
-      stilts_was_above_ = false;
-    }
-
     if (switch_ball_) {
       if (superstructure_queue.status->has_piece) {
         new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
@@ -342,6 +313,33 @@
       }
     }
 
+    constexpr double kDeployStiltPosition = 0.5;
+
+    // TODO(sabina): max height please?
+    if (data.IsPressed(kFallOver)) {
+      new_superstructure_goal->stilts.unsafe_goal = 0.71;
+      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+      new_superstructure_goal->stilts.profile_params.max_acceleration = 1.25;
+    } else if (data.IsPressed(kDeployStilt)) {
+      new_superstructure_goal->stilts.unsafe_goal = kDeployStiltPosition;
+      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+      new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+    } else if (data.IsPressed(kHalfStilt)) {
+      new_superstructure_goal->stilts.unsafe_goal = 0.345;
+      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+    } else {
+      new_superstructure_goal->stilts.unsafe_goal = 0.005;
+      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+      new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+    }
+
+    if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
+        new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
+      new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
+      new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
+    }
+
+
     if (data.IsPressed(kRelease)) {
       grab_piece_ = false;
     }
@@ -378,7 +376,6 @@
   bool grab_piece_ = false;
 
   bool switch_ball_ = false;
-  bool stilts_was_above_ = false;
 
   VisionControl vision_control_;
   ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;