Working on various auto lock on and some Auto stuff

src/main/java/frc/robot/constants/AutoConstants.java

@@ -10,6 +10,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.PathConstraints;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 
 // TODO This is all hold over from 2025, does any of it need to change?
 public class AutoConstants {
@@ -23,6 +24,8 @@ public class AutoConstants {
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 
+    public static final double kYawPIDTolerance = Units.degreesToRadians(2);
+
     public static final double kAlignPXYController = 2;
     public static final double kAlignPThetaController = 5;
 

src/main/java/frc/robot/constants/CompetitionConstants.java

@@ -2,6 +2,9 @@ package frc.robot.constants;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 
 public class CompetitionConstants {
     // THIS SHOULD BE FALSE DURING COMPETITION PLAY
@@ -10,4 +13,17 @@ public class CompetitionConstants {
     public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
         AprilTagFields.kDefaultField
     );
+
+    public static final Pose2d kBlueHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11), 
+        Units.inchesToMeters(158.84), 
+        Rotation2d.fromDegrees(0)
+    );
+
+    public static final Pose2d kRedHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11 + 143.5 * 2),
+        Units.inchesToMeters(158.84),
+        Rotation2d.fromDegrees(0)
+    );
+
 }