commit d76546a48fe57da23c6a72603c5de873de94f672
author Austin Schuh <austin.linux@gmail.com> Sun Jul 08 16:05:14 2018 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Aug 08 20:39:17 2018 -0700
tree a2ac8c7c38594db4a976d7e5e36ede9591b1ac10
parent ede4732ae681c0e870c07ce038b9f57cc7d6f8cc

Add throw and override modes for the Arm

This lets us throw boxes in teleop, and also get unstuck from under or
over the scale

Change-Id: Ia71824d8ae5801795d7ea21ce1becdfc1d2b55ee

y2018/joystick_reader.cc

diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 61e3d9f..173e6ef 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -66,6 +66,9 @@
 const ButtonLocation kArmAboveHang(3, 15);
 const ButtonLocation kArmBelowHang(3, 16);
 
+const ButtonLocation kEmergencyUp(3, 5);
+const ButtonLocation kEmergencyDown(3, 6);
+
 const ButtonLocation kWinch(4, 2);
 
 const ButtonLocation kArmNeutral(3, 8);
@@ -297,6 +300,16 @@
       arm_goal_position_ = arm::BelowHangIndex();
     }
 
+    if (data.IsPressed(kEmergencyDown)) {
+      arm_goal_position_ = arm::NeutralIndex();
+      new_superstructure_goal->trajectory_override = true;
+    } else if (data.IsPressed(kEmergencyUp)) {
+      arm_goal_position_ = arm::UpIndex();
+      new_superstructure_goal->trajectory_override = true;
+    } else {
+      new_superstructure_goal->trajectory_override = false;
+    }
+
     new_superstructure_goal->deploy_fork =
         data.IsPressed(kArmAboveHang) && data.IsPressed(kClawOpen);
 
@@ -314,9 +327,16 @@
     new_superstructure_goal->hook_release = data.IsPressed(kArmBelowHang);
 
     new_superstructure_goal->arm_goal_position = arm_goal_position_;
+    if (data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)) {
+      new_superstructure_goal->open_threshold = 0.35;
+    } else {
+      new_superstructure_goal->open_threshold = 0.0;
+    }
 
     if ((data.IsPressed(kClawOpen) && data.IsPressed(kDriverClawOpen)) ||
-        data.PosEdge(kArmPickupBoxFromIntake)) {
+        data.PosEdge(kArmPickupBoxFromIntake) ||
+        (data.IsPressed(kClawOpen) &&
+         (data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)))) {
       new_superstructure_goal->open_claw = true;
     } else {
       new_superstructure_goal->open_claw = false;