From 67ddbc09c02775aec5d90bb6837c0e1b59182609 Mon Sep 17 00:00:00 2001
From: mwitcpalek <mwitcpalek@gmail.com>
Date: Fri, 11 Jul 2025 17:03:57 -0400
Subject: [PATCH 1/3] add new steal auto, adjust pole offsets

---
 .../robot/constants/TagServoingConstants.java | 30 ++++++++++---------
 .../frc/robot/subsystems/auto/AutoSwitch.java | 17 +++++++++++
 2 files changed, 33 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java
index b4c422ed..c72021ab 100644
--- a/src/main/java/frc/robot/constants/TagServoingConstants.java
+++ b/src/main/java/frc/robot/constants/TagServoingConstants.java
@@ -43,20 +43,20 @@ public class TagServoingConstants {
 
   // Offsets (left is negative, right is pos)
   public static final double[][][] kBlueCoralOffset = {
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}}
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}}
   }; // Alliance relative hexant, level, left/right
   public static final double[][][] kRedCoralOffset = {
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
-    {{0, 0}, {0, 0}, {0, 0}, {0, 0}}
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
+    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}}
   }; // Alliance relative hexant, level, left/right
 
   // Tag align
@@ -101,6 +101,8 @@ public class TagServoingConstants {
   public static final double kCoralStuckAllowence = 0.03;
 
   // Reef
-  public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259);
-  public static final Translation2d kRedReefPose = new Translation2d(13.058902, 4.0259);
+  public static final Translation2d kBlueReefPose =
+      new Translation2d(4.489323, 4.02082); // welded: 4.489323, 4.0259
+  public static final Translation2d kRedReefPose =
+      new Translation2d(13.058902, 4.02082); // welded: 13.058902, 4.0259
 }
diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
index 2532d80a..3b6a052d 100644
--- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
+++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
@@ -501,6 +501,23 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             ScoringLevel.L2,
             new Pose2d(7.1008875, 4.0509, Rotation2d.fromDegrees(180.0)));
       }
+      case 0x18 -> {
+        return new StealOneAlgeaAutonCommand(
+            driveSubsystem,
+            robotStateSubsystem,
+            algaeSubsystem,
+            biscuitSubsystem,
+            coralSubsystem,
+            elevatorSubsystem,
+            tagAlignSubsystem,
+            visionSubsystem,
+            "startBargeToG",
+            "GToBarge",
+            "bargeToOppG",
+            "OppGToOppbarge",
+            ScoringLevel.L2,
+            new Pose2d(7.1, 3.7209, Rotation2d.fromRadians(3.14159)));
+      }
 
       case 0x20 -> {
         return new ProcessorShallowSlowAutonCommand(

From 294f2872194ce6a49bfff65003d294c01b617ad0 Mon Sep 17 00:00:00 2001
From: mwitcpalek <mwitcpalek@gmail.com>
Date: Sat, 12 Jul 2025 07:48:11 -0400
Subject: [PATCH 2/3] fix typo

---
 src/main/java/frc/robot/subsystems/auto/AutoSwitch.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
index 3b6a052d..7460c174 100644
--- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
+++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
@@ -514,7 +514,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             "startBargeToG",
             "GToBarge",
             "bargeToOppG",
-            "OppGToOppbarge",
+            "OppGToOppBarge",
             ScoringLevel.L2,
             new Pose2d(7.1, 3.7209, Rotation2d.fromRadians(3.14159)));
       }

From d953eff6ecb5bab3269ca2aee62f81f0c85e35ff Mon Sep 17 00:00:00 2001
From: mwitcpalek <mwitcpalek@gmail.com>
Date: Tue, 15 Jul 2025 18:07:50 -0400
Subject: [PATCH 3/3] revert iri changes (am field)

---
 .../robot/constants/TagServoingConstants.java | 28 +++++++++----------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java
index c72021ab..0061dc88 100644
--- a/src/main/java/frc/robot/constants/TagServoingConstants.java
+++ b/src/main/java/frc/robot/constants/TagServoingConstants.java
@@ -43,20 +43,20 @@ public class TagServoingConstants {
 
   // Offsets (left is negative, right is pos)
   public static final double[][][] kBlueCoralOffset = {
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}}
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}}
   }; // Alliance relative hexant, level, left/right
   public static final double[][][] kRedCoralOffset = {
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}},
-    {{-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}, {-0.00635, -0.00635}}
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
+    {{0, 0}, {0, 0}, {0, 0}, {0, 0}}
   }; // Alliance relative hexant, level, left/right
 
   // Tag align
@@ -102,7 +102,7 @@ public class TagServoingConstants {
 
   // Reef
   public static final Translation2d kBlueReefPose =
-      new Translation2d(4.489323, 4.02082); // welded: 4.489323, 4.0259
+      new Translation2d(4.489323, 4.0259); // welded: 4.489323, 4.0259, AM: 4.489323, 4.02082
   public static final Translation2d kRedReefPose =
-      new Translation2d(13.058902, 4.02082); // welded: 13.058902, 4.0259
+      new Translation2d(13.058902, 4.0259); // welded: 13.058902, 4.0259, AM: 13.058902, 4.02082
 }