Climb Autoalign

src/main/java/frc/robot/Robot.java

@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -49,6 +50,9 @@
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread;
 import frc.robot.utils.CommandXboxControllerSubsystem;
+import frc.robot.utils.FieldUtils;
+import frc.robot.utils.FieldUtils.ClimbTargets;
+import java.util.Arrays;
 import java.util.Optional;
 import java.util.Set;
 import org.ironmaple.simulation.SimulatedArena;
@@ -75,6 +79,9 @@ public enum RobotEdition {
     COMP
   }
 
+  @AutoLogOutput(key = "Robot/Climb Target")
+  private boolean leftClimbTarget = true;
+
   public static final RobotMode ROBOT_MODE = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM;
   // public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP;
   public static final RobotEdition ROBOT_EDITION;
@@ -532,6 +539,29 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
 
     new Trigger(() -> intake.beambreak()).onTrue(driver.rumbleCmd(1, 1).withTimeout(0.5));
 
+    // current zero shooter hood
+    driver.b().whileTrue(shooter.runCurrentZeroing());
+
+    new Trigger(() -> intake.beambreak()).onTrue(driver.rumbleCmd(1, 1).withTimeout(0.5));
+
+    operator.leftBumper().onTrue(Commands.runOnce(() -> leftClimbTarget = true));
+    operator.rightBumper().onTrue(Commands.runOnce(() -> leftClimbTarget = false));
+
+    // TODO: ACTUAL BINDING LOL
+    driver
+        .x()
+        .whileTrue(
+            swerve.alignToClimb(
+                () ->
+                    ClimbTargets.CLIMB_TARGETS_LIST.stream()
+                        .filter(target -> target.getLeftHanded() == leftClimbTarget)
+                        .filter(
+                            target ->
+                                target.isBlueAlliance()
+                                    == (DriverStation.getAlliance().orElse(Alliance.Blue)
+                                        == Alliance.Blue))
+                        .findFirst()
+                        .get()));
     // ---zeroing stuff---
 
     // create triggers for joystick disconnect alerts
@@ -567,6 +597,13 @@ public void robotPeriodic() {
     // TODO Log mechanism poses
 
     updateAlerts();
+
+    // Log climb poses
+    Logger.recordOutput(
+        "AutoAlign/Climb Targets",
+        Arrays.stream(FieldUtils.ClimbTargets.values())
+            .map(target -> target.getPose())
+            .toArray(Pose2d[]::new));
   }
 
   public void updateAlerts() {

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

@@ -50,6 +50,7 @@
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.SignalID;
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.SignalType;
 import frc.robot.utils.FieldUtils;
+import frc.robot.utils.FieldUtils.ClimbTargets;
 import frc.robot.utils.FieldUtils.FeedTargets;
 import frc.robot.utils.Tracer;
 import frc.robot.utils.autoaim.AutoAim;
@@ -579,6 +580,11 @@ private Command translateWithIntermediatePose(
         .andThen(translateToPose(target));
   }
 
+  public Command alignToClimb(Supplier<ClimbTargets> target) {
+    // TODO: Might need tolerance
+    return translateToPose(() -> target.get().getPose());
+  }
+
   private Command driveWithHeadingSnap(
       Supplier<Rotation2d> target, DoubleSupplier xVel, DoubleSupplier yVel) {
     return Commands.runOnce(

src/main/java/frc/robot/utils/FieldUtils.java

@@ -8,9 +8,12 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Superstructure.FeedTarget;
+import java.util.Arrays;
+import java.util.List;
 
 /** Add your docs here. */
 public class FieldUtils {
@@ -65,4 +68,44 @@ public static FeedTargets getFeedTarget(FeedTarget target) {
       }
     }
   }
+
+  public enum ClimbTargets {
+    // Grabbed in Choreo. Needs real testing
+    // 3.05 is climber offset
+    // TODO: VERIFY POSES
+    BLUE_RIGHT(
+        new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg), false, true),
+    BLUE_LEFT(
+        // x2 offset because we need to offset again after rotating 180 deg
+        new Pose2d(1.189 - (Units.inchesToMeters(3.05) * 2), 2.845, Rotation2d.kCW_90deg),
+        true,
+        true),
+    RED_RIGHT(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT.getPose()), false, false),
+    RED_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_LEFT.getPose()), true, false);
+
+    private Pose2d targetPose;
+    private boolean leftHanded;
+    private boolean isBlueAlliance;
+
+    private ClimbTargets(Pose2d pose, boolean leftHanded, boolean isBlueAlliance) {
+      this.targetPose = pose;
+      this.leftHanded = leftHanded;
+      this.isBlueAlliance = isBlueAlliance;
+    }
+
+    public Pose2d getPose() {
+      return targetPose;
+    }
+
+    public boolean getLeftHanded() {
+      return leftHanded;
+    }
+
+    public boolean isBlueAlliance() {
+      return isBlueAlliance;
+    }
+
+    public static final List<ClimbTargets> CLIMB_TARGETS_LIST =
+        Arrays.stream(ClimbTargets.values()).toList();
+  }
 }