add new steal auto, adjust pole offsets

Author: mwitcpalek

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
 }

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(