commit d8deb681b9d4a41cddfe40586126b5bf1158a82c
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 10 10:38:42 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 10 12:08:24 2019 -0700
tree 9bebf954020f143e9b0d9676c3968083b1aa0d78
parent 46f3a21aa25bcab91e0c97cc1cf9e540c4e53fab

Add button for resetting the localizer to HP slot

Change-Id: Ic3d7ea4304c8e732ea62d9b9ce1574b41ba5a87e

y2019/joystick_reader.cc

diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 76ded48..7f9fe6c 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -15,12 +15,14 @@
 #include "frc971/autonomous/auto.q.h"
 #include "frc971/autonomous/base_autonomous_actor.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/localizer.q.h"
 
 #include "y2019/control_loops/drivetrain/drivetrain_base.h"
 #include "y2019/control_loops/superstructure/superstructure.q.h"
 #include "y2019/status_light.q.h"
 
 using ::y2019::control_loops::superstructure::superstructure_queue;
+using ::frc971::control_loops::drivetrain::localizer_control;
 using ::aos::input::driver_station::ButtonLocation;
 using ::aos::input::driver_station::ControlBit;
 using ::aos::input::driver_station::JoystickAxis;
@@ -64,6 +66,7 @@
 const ButtonLocation kPanelHPIntakeBackward(5, 5);
 
 const ButtonLocation kRelease(2, 4);
+const ButtonLocation kResetLocalizer(4, 3);
 
 const ElevatorWristPosition kStowPos{0.36, 0.0};
 
@@ -122,6 +125,16 @@
 
     auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
 
+    if (data.PosEdge(kResetLocalizer)) {
+      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.IsPressed(kSuctionBall)) {
       grab_piece_ = true;
     } else if (data.IsPressed(kSuctionHatch)) {