cleaned up align logic

Team334 · Feb 8, 2025 · 2e0ad97 · 2e0ad97
1 parent ecd09d6
commit 2e0ad97
Showing 1 changed file with 21 additions and 33 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -443,26 +443,28 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) {
               Pose2d pose = getPose();
               Optional<Alliance> alliance = DriverStation.getAlliance();
 
+              AlignPoses rotated =
+                  alliance.get() == Alliance.Red
+                      ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
+                      : alignGoal;
+
+              _alignGoal = rotated;
+
               if (alignGoal == FieldConstants.reef) {
                 double minDistance = Double.MAX_VALUE;
 
-                var goal =
-                    alliance.get() == Alliance.Red
-                        ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
-                        : alignGoal;
-
                 var reefCenter =
                     alliance.get() == Alliance.Red
                         ? FieldConstants.reefCenter.rotateAround(
                             FieldConstants.fieldCenter, Rotation2d.k180deg)
                         : FieldConstants.reefCenter;
 
                 for (int i = 0; i < 6; i++) {
-                  var rotated = goal.rotateAround(reefCenter, Rotation2d.fromDegrees(60).times(i));
+                  var goal = rotated.rotateAround(reefCenter, Rotation2d.fromDegrees(60).times(i));
 
-                  if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) {
-                    _alignGoal = rotated;
-                    minDistance = pose.minus(rotated.getCenter()).getTranslation().getNorm();
+                  if (pose.minus(goal.getCenter()).getTranslation().getNorm() < minDistance) {
+                    _alignGoal = goal;
+                    minDistance = pose.minus(goal.getCenter()).getTranslation().getNorm();
                   }
                 }
               }
@@ -471,39 +473,25 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) {
                 double minDistance = Double.MAX_VALUE;
 
                 for (int i = 0; i < 2; i++) {
-                  AlignPoses rotated = alignGoal.offset(0, -6.26 * i);
+                  AlignPoses offset = alignGoal.offset(0, -6.26 * i);
 
-                  rotated =
+                  offset =
                       alliance.get() == Alliance.Red
-                          ? rotated.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
-                          : rotated;
+                          ? offset.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
+                          : offset;
 
-                  rotated =
-                      rotated.rotateAround(
-                          rotated.getCenter().getTranslation(),
+                  offset =
+                      offset.rotateAround(
+                          offset.getCenter().getTranslation(),
                           Rotation2d.fromDegrees(106).times(i));
 
-                  if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) {
-                    _alignGoal = rotated;
-                    minDistance = pose.minus(rotated.getCenter()).getTranslation().getNorm();
+                  if (pose.minus(offset.getCenter()).getTranslation().getNorm() < minDistance) {
+                    _alignGoal = offset;
+                    minDistance = pose.minus(offset.getCenter()).getTranslation().getNorm();
                   }
                 }
               }
 
-              if (alignGoal == FieldConstants.processor) {
-                _alignGoal =
-                    alliance.get() == Alliance.Red
-                        ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
-                        : alignGoal;
-              }
-
-              if (alignGoal == FieldConstants.cage) {
-                _alignGoal =
-                    alliance.get() == Alliance.Red
-                        ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
-                        : alignGoal;
-              }
-
               DogLog.log("Auto/Align Pose", _alignGoal.getPose(side));
             })
         .andThen(defer(() -> driveTo(_alignGoal.getPose(side))))