commit 7fc03cecb19e267f8cb921a05b979ded1ba84bbc
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 15:31:24 2022 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 15:32:58 2022 -0800
tree d4ecde60152f20dc941ea3ee4490fe983057c834
parent fe09a45f163fd30f2e651d5f918c649f1f67690e

Fixup localizer reset positions

Red & Blue were swapped, and didn't match.

Change-Id: I269b4bb2ae549608246f72482a4923fa58e4ac8e
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

y2022/joystick_reader.cc

diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 4b2f082..2f707d7 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -60,8 +60,8 @@
 const ButtonLocation kIntakeFrontOut(4, 10);
 const ButtonLocation kIntakeBackOut(4, 9);
 
-const ButtonLocation kRedLocalizerReset(4, 13);
-const ButtonLocation kBlueLocalizerReset(4, 14);
+const ButtonLocation kRedLocalizerReset(4, 14);
+const ButtonLocation kBlueLocalizerReset(4, 13);
 const ButtonLocation kLocalizerReset(3, 8);
 #endif
 
@@ -83,16 +83,18 @@
         setpoint_fetcher_(
             event_loop->MakeFetcher<Setpoint>("/superstructure")) {}
 
+  // Localizer reset positions are with front of robot pressed up against driver
+  // station in the middle of the field side-to-side.
   void BlueResetLocalizer() {
     auto builder = localizer_control_sender_.MakeBuilder();
 
     frc971::control_loops::drivetrain::LocalizerControl::Builder
         localizer_control_builder = builder.MakeBuilder<
             frc971::control_loops::drivetrain::LocalizerControl>();
-    localizer_control_builder.add_x(7.4);
-    localizer_control_builder.add_y(-1.7);
+    localizer_control_builder.add_x(-7.9);
+    localizer_control_builder.add_y(0.0);
     localizer_control_builder.add_theta_uncertainty(10.0);
-    localizer_control_builder.add_theta(0.0);
+    localizer_control_builder.add_theta(M_PI);
     localizer_control_builder.add_keep_current_theta(false);
     if (builder.Send(localizer_control_builder.Finish()) !=
         aos::RawSender::Error::kOk) {
@@ -106,10 +108,10 @@
     frc971::control_loops::drivetrain::LocalizerControl::Builder
         localizer_control_builder = builder.MakeBuilder<
             frc971::control_loops::drivetrain::LocalizerControl>();
-    localizer_control_builder.add_x(-7.9);
-    localizer_control_builder.add_y(0.7);
+    localizer_control_builder.add_x(7.9);
+    localizer_control_builder.add_y(0.0);
     localizer_control_builder.add_theta_uncertainty(10.0);
-    localizer_control_builder.add_theta(M_PI);
+    localizer_control_builder.add_theta(0.0);
     localizer_control_builder.add_keep_current_theta(false);
     if (builder.Send(localizer_control_builder.Finish()) !=
         aos::RawSender::Error::kOk) {