commit e2f2248b2e032cd57ce2971e0f25fe2c98a23741
author Austin Schuh <austin.linux@gmail.com> Sat Apr 13 23:05:43 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Apr 13 23:10:05 2019 -0700
tree 56c64f0ca65e74168af6bd5a729fb1dabc4e5cab
parent ce2885dbf3fd667df13185a6087e78549602d289

Require the robot to see the platform before retracting the stilts

If you hit the level 2 button, it will force retract them.

Change-Id: I8d16cd3c06769043c7a5fd50de50aa1d6ac2e3d8

y2019/joystick_reader.cc

diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index a82079b..d024d3f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -158,7 +158,7 @@
     superstructure_queue.status.FetchLatest();
     if (!superstructure_queue.status.get() ||
         !superstructure_queue.position.get()) {
-      LOG(ERROR, "Got no superstructure status packet.\n");
+      LOG(ERROR, "Got no superstructure status or position packet.\n");
       return;
     }
 
@@ -342,18 +342,18 @@
 
     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)) {
+    } else if (data.IsPressed(kHalfStilt)) {
+      was_above_ = false;
+      new_superstructure_goal->stilts.unsafe_goal = 0.345;
+      new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+    } else if (data.IsPressed(kDeployStilt) || was_above_) {
       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;
@@ -366,6 +366,18 @@
       new_superstructure_goal->elevator.profile_params.max_acceleration = 6;
     }
 
+    // If we've been asked to go above deploy and made it up that high, latch
+    // was_above.
+    if (new_superstructure_goal->stilts.unsafe_goal > kDeployStiltPosition &&
+        superstructure_queue.status->stilts.position >= kDeployStiltPosition) {
+      was_above_ = true;
+    } else if ((superstructure_queue.position->platform_left_detect ||
+                superstructure_queue.position->platform_right_detect) &&
+               !data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
+      // TODO(austin): Should make this && rather than ||
+      was_above_ = false;
+    }
+
     if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
         new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
       new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
@@ -419,6 +431,10 @@
     return ::frc971::autonomous::auto_mode->mode;
   }
 
+  // Bool to track if we've been above the deploy position.  Once this bool is
+  // set, don't let the stilts retract until we see the platform.
+  bool was_above_ = false;
+
   // Current goals here.
   ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
   bool grab_piece_ = false;