commit bb1df6ffd9c456148e72cebf37ee86d9bb250805
author Austin Schuh <austin.linux@gmail.com> Sun Mar 17 18:15:43 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Fri Mar 22 20:03:18 2019 -0700
tree bdeb7d92d05b2a5d453c74c30e3ffb82f1764394
parent 9c03a53edfcdcdb3d6f985924ff00b6544f2436b

Add 4 localizer reset buttons for each orientation in each HP station

Change-Id: I61a8d27722ba2c4806d8780c693af0bee02a5906

y2019/joystick_reader.cc

diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 23bc188..f54511b 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -76,13 +76,11 @@
 const ButtonLocation kPanelHPIntakeBackward(5, 5);
 
 const ButtonLocation kRelease(2, 4);
-const ButtonLocation kResetLocalizer(4, 3);
+const ButtonLocation kResetLocalizerLeftForwards(3, 14);
+const ButtonLocation kResetLocalizerLeftBackwards(4, 12);
 
-const ButtonLocation kAutoPanel(3, 10);
-const ButtonLocation kAutoPanelIntermediate(4, 6);
-
-const ElevatorWristPosition kAutoPanelPos{0.0, -M_PI / 2.0};
-const ElevatorWristPosition kAutoPanelIntermediatePos{0.34, -M_PI / 2.0};
+const ButtonLocation kResetLocalizerRightForwards(4, 1);
+const ButtonLocation kResetLocalizerRightBackwards(4, 11);
 
 const ElevatorWristPosition kStowPos{0.36, 0.0};
 
@@ -131,6 +129,7 @@
     superstructure_queue.goal.FetchLatest();
     if (superstructure_queue.goal.get()) {
       grab_piece_ = superstructure_queue.goal->suction.grab_piece;
+      switch_ball_ = superstructure_queue.goal->suction.gamepiece_mode == 0;
     }
     video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
         StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
@@ -159,11 +158,49 @@
 
     auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
 
-    if (data.PosEdge(kResetLocalizer)) {
+    if (data.PosEdge(kResetLocalizerLeftForwards)) {
       auto localizer_resetter = localizer_control.MakeMessage();
       localizer_resetter->x = 0.4;
       localizer_resetter->y = 3.4;
       localizer_resetter->theta = 0.0;
+
+      if (!localizer_resetter.Send()) {
+        LOG(ERROR, "Failed to reset localizer.\n");
+      }
+    }
+
+    if (data.PosEdge(kResetLocalizerLeftBackwards)) {
+      auto localizer_resetter = localizer_control.MakeMessage();
+      // Start at the left feeder station.
+      localizer_resetter->x = 0.4;
+      localizer_resetter->y = 3.4;
+      localizer_resetter->theta = M_PI;
+
+      if (!localizer_resetter.Send()) {
+        LOG(ERROR, "Failed to reset localizer.\n");
+      }
+    }
+
+    // TODO(austin): Check y location.
+    if (data.PosEdge(kResetLocalizerRightForwards)) {
+      auto localizer_resetter = localizer_control.MakeMessage();
+      // Start at the left feeder station.
+      localizer_resetter->x = 0.4;
+      localizer_resetter->y = -3.4;
+      localizer_resetter->theta = 0.0;
+
+      if (!localizer_resetter.Send()) {
+        LOG(ERROR, "Failed to reset localizer.\n");
+      }
+    }
+
+    if (data.PosEdge(kResetLocalizerRightBackwards)) {
+      auto localizer_resetter = localizer_control.MakeMessage();
+      // Start at the left feeder station.
+      localizer_resetter->x = 0.4;
+      localizer_resetter->y = -3.4;
+      localizer_resetter->theta = M_PI;
+
       if (!localizer_resetter.Send()) {
         LOG(ERROR, "Failed to reset localizer.\n");
       }
@@ -222,12 +259,6 @@
       stilts_was_above_ = false;
     }
 
-    if (data.IsPressed(kAutoPanel)) {
-      elevator_wrist_pos_ = kAutoPanelPos;
-    } else if (data.IsPressed(kAutoPanelIntermediate)) {
-      elevator_wrist_pos_ = kAutoPanelIntermediatePos;
-    }
-
     if (switch_ball_) {
       if (superstructure_queue.status->has_piece) {
         new_superstructure_goal->wrist.profile_params.max_acceleration = 20;