add auto align offset

Author: rift10

src/main/java/frc/robot/RobotState.java

@@ -40,4 +40,9 @@ public static RobotState getInstance() {
     @Getter
     @AutoLogOutput(key = "RobotState/Elevator Level")
     private ElevatorGoal elevatorLevel;
+
+    @Setter
+    @Getter
+    @AutoLogOutput(key = "RobotState/Is early")
+    private boolean isEarly;
 }

src/main/java/frc/robot/subsystems/swerve/Swerve.java

@@ -37,13 +37,15 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.RobotState;
 import frc.robot.constants.FieldConstants;
 import frc.robot.constants.GlobalConstants;
 import frc.robot.constants.GlobalConstants.Mode;
@@ -386,7 +388,12 @@ public Pose2d getBestReefTagNoOffset() {
     }
 
     public Pose2d getBestCoralAutoAlign() {
-        Translation2d offset = new Translation2d();
+        Translation2d offset = RobotState.getInstance().isEarly()
+                ? FieldConstants.CoralTags.kCoralAlignOffset.plus(
+                        new Translation2d(-0.025, 0.05 + .0175 + Units.inchesToMeters(1.5)))
+                : FieldConstants.CoralTags.kCoralAlignOffset
+                        .plus(new Translation2d(0.025, 0.142))
+                        .unaryMinus();
         return getBestCoralTag()
                 .plus(GeomUtil.toTransform2d(offset))
                 .plus(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180)));

src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java

@@ -1,7 +1,6 @@
 package frc.robot.subsystems.swerve;
 
 import static edu.wpi.first.units.Units.MetersPerSecond;
-import static frc.robot.subsystems.swerve.SwerveConstants.TunerConstants;
 
 import com.ctre.phoenix6.CANBus;
 import com.pathplanner.lib.config.ModuleConfig;
@@ -15,10 +14,9 @@
 
 public class SwerveConstants {
 
-    /** <b> !! PLACEHOLDER VALUE !! </b> */
     public static final ConstantsWrapper TunerConstants = new ConstantsWrapper(TunerConstantsV3.class);
 
-    // TunerConstantsPlaceholder doesn't include these constants, so they are declared locally
+    // TunerConstants doesn't include these constants, so they are declared locally
 
     public static final double kOdometryFreq =
             new CANBus(TunerConstants.getDrivetrainConstants().CANBusName).isNetworkFD() ? 250.0 : 100.0;
@@ -30,6 +28,7 @@ public class SwerveConstants {
                     Math.hypot(TunerConstants.getBackLeft().LocationX, TunerConstants.getBackLeft().LocationY),
                     Math.hypot(TunerConstants.getBackRight().LocationX, TunerConstants.getBackRight().LocationY)));
 
+    // TODO: set
     public static final double kRobotMass = 63.957;
     public static final double kRobotMoi = 3.825;
     public static final double kWheelCOF = 1.9;
@@ -51,6 +50,8 @@ public class SwerveConstants {
             getModuleTranslations());
 
     public static final PPHolonomicDriveController kAutoController =
+            // translation pids, rotation pids
+            // TODO: set
             new PPHolonomicDriveController(new PIDConstants(2, 0.0, 0), new PIDConstants(2, 0.0, 0.0));
 
     public static Translation2d[] getModuleTranslations() {