From 9b3211db88c7af2b0f022617f4bbd600a206a880 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 11:43:54 -0800 Subject: [PATCH 1/6] Add poses in field utils --- src/main/java/frc/robot/utils/FieldUtils.java | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index d5db691..5af138c 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -8,6 +8,7 @@ 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; @@ -52,4 +53,24 @@ public Translation2d getTranslation() { return targetPose.getTranslation(); } } + + public enum ClimbTargets { + // Grabbed in Choreo. Needs real testing + // 3.05 is climber offset + BLUE_RIGHT(new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg)), + BLUE_LEFT(new Pose2d(1.189 - Units.inchesToMeters(3.05) ,2.845, Rotation2d.kCW_90deg)), + RED_RIGHT(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT.getPose())), + RED_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_LEFT.getPose())) + ; + + private Pose2d targetPose; + + private ClimbTargets(Pose2d pose) { + this.targetPose = pose; + } + + public Pose2d getPose() { + return targetPose; + } + } } From aea8904b7e7a96100d837f3c7b06d2cc84cbd5da Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 11:51:49 -0800 Subject: [PATCH 2/6] Log poses --- src/main/java/frc/robot/Robot.java | 10 ++++++++++ src/main/java/frc/robot/utils/FieldUtils.java | 5 ++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 01d26b8..e0063bf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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; @@ -48,6 +49,8 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.FieldUtils; +import java.util.Arrays; import java.util.Optional; import java.util.Set; import org.ironmaple.simulation.SimulatedArena; @@ -532,6 +535,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() { diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index 5af138c..7833a5e 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -58,10 +58,9 @@ public enum ClimbTargets { // Grabbed in Choreo. Needs real testing // 3.05 is climber offset BLUE_RIGHT(new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg)), - BLUE_LEFT(new Pose2d(1.189 - Units.inchesToMeters(3.05) ,2.845, Rotation2d.kCW_90deg)), + BLUE_LEFT(new Pose2d(1.189 + Units.inchesToMeters(3.05), 2.845, Rotation2d.kCW_90deg)), RED_RIGHT(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT.getPose())), - RED_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_LEFT.getPose())) - ; + RED_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_LEFT.getPose())); private Pose2d targetPose; From 306bbd69fe4d58547c806fabcc2bfcd5513bf981 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 12:33:04 -0800 Subject: [PATCH 3/6] Add button binding for climb autoalign --- src/main/java/frc/robot/Robot.java | 22 +++++++++++++++ .../subsystems/swerve/SwerveSubsystem.java | 7 +++++ src/main/java/frc/robot/utils/FieldUtils.java | 28 +++++++++++++++---- 3 files changed, 52 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e0063bf..1c8e5e6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,6 +50,8 @@ 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; @@ -77,6 +79,8 @@ 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; @@ -500,6 +504,24 @@ private void addControllerBindings() { * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); // TODO add binding for climb + 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 diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 1433bca..30050ee 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.Robot.ClimbTarget; import frc.robot.Robot.RobotEdition; import frc.robot.Robot.RobotMode; import frc.robot.components.camera.Camera; @@ -50,6 +51,7 @@ import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.SignalType; import frc.robot.utils.FieldUtils; import frc.robot.utils.Tracer; +import frc.robot.utils.FieldUtils.ClimbTargets; import frc.robot.utils.autoaim.AutoAlign; import java.util.Arrays; import java.util.List; @@ -577,6 +579,11 @@ private Command translateWithIntermediatePose( .andThen(translateToPose(target)); } + public Command alignToClimb(Supplier target) { + // TODO: Might need tolerance + return translateToPose(() -> target.get().getPose()); + } + private Command driveWithHeadingSnap( Supplier target, DoubleSupplier xVel, DoubleSupplier yVel) { return Commands.runOnce( diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index 7833a5e..e231519 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -4,6 +4,9 @@ package frc.robot.utils; +import java.util.Arrays; +import java.util.List; + import choreo.util.ChoreoAllianceFlipUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -57,19 +60,34 @@ public Translation2d getTranslation() { public enum ClimbTargets { // Grabbed in Choreo. Needs real testing // 3.05 is climber offset - BLUE_RIGHT(new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg)), - BLUE_LEFT(new Pose2d(1.189 + Units.inchesToMeters(3.05), 2.845, Rotation2d.kCW_90deg)), - RED_RIGHT(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT.getPose())), - RED_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_LEFT.getPose())); + // TODO: VERIFY POSES + BLUE_RIGHT(new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg), false, true), + BLUE_LEFT(new Pose2d(1.189 + Units.inchesToMeters(3.05), 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) { + 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 CLIMB_TARGETS_LIST = Arrays.stream(ClimbTargets.values()).toList(); } } From de2043e387aceb359c40b7585da275a219084737 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 12:37:04 -0800 Subject: [PATCH 4/6] Fmt and fix filters --- src/main/java/frc/robot/Robot.java | 34 +++++++++---------- .../subsystems/swerve/SwerveSubsystem.java | 3 +- src/main/java/frc/robot/utils/FieldUtils.java | 14 ++++---- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1c8e5e6..54f84bd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -51,7 +51,6 @@ 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; @@ -79,7 +78,8 @@ public enum RobotEdition { COMP } - @AutoLogOutput(key = "Robot/Climb Target") private boolean leftClimbTarget = true; + @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; @@ -504,24 +504,24 @@ private void addControllerBindings() { * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); // TODO add binding for climb - operator - .leftBumper() - .onTrue(Commands.runOnce(() -> leftClimbTarget = true)); - operator - .rightBumper() - .onTrue(Commands.runOnce(() -> leftClimbTarget = false)); + 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() - )); + .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 diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 30050ee..96757aa 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -27,7 +27,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.Robot.ClimbTarget; import frc.robot.Robot.RobotEdition; import frc.robot.Robot.RobotMode; import frc.robot.components.camera.Camera; @@ -50,8 +49,8 @@ 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.Tracer; import frc.robot.utils.FieldUtils.ClimbTargets; +import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAlign; import java.util.Arrays; import java.util.List; diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index e231519..e57ff39 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -4,9 +4,6 @@ package frc.robot.utils; -import java.util.Arrays; -import java.util.List; - import choreo.util.ChoreoAllianceFlipUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -14,6 +11,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Arrays; +import java.util.List; /** Add your docs here. */ public class FieldUtils { @@ -61,8 +60,10 @@ 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(new Pose2d(1.189 + Units.inchesToMeters(3.05), 2.845, Rotation2d.kCW_90deg), true, true), + BLUE_RIGHT( + new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg), false, true), + BLUE_LEFT( + new Pose2d(1.189 + Units.inchesToMeters(3.05), 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); @@ -88,6 +89,7 @@ public boolean isBlueAlliance() { return isBlueAlliance; } - public static final List CLIMB_TARGETS_LIST = Arrays.stream(ClimbTargets.values()).toList(); + public static final List CLIMB_TARGETS_LIST = + Arrays.stream(ClimbTargets.values()).toList(); } } From 0cba43cda7a6d0e293baaf5869ff1ab8432a49c1 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 17:23:10 -0800 Subject: [PATCH 5/6] Fix target poses using ascope viz --- src/main/java/frc/robot/utils/FieldUtils.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index e57ff39..84a3513 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -63,7 +63,8 @@ public enum ClimbTargets { BLUE_RIGHT( new Pose2d(1.189 - Units.inchesToMeters(3.05), 4.658, Rotation2d.kCCW_90deg), false, true), BLUE_LEFT( - new Pose2d(1.189 + Units.inchesToMeters(3.05), 2.845, Rotation2d.kCW_90deg), true, true), + // 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); From f3c27e2c84faa5a88c2f992fd56bfb3b7d8ea9d8 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 12:49:54 -0800 Subject: [PATCH 6/6] Spotless --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- src/main/java/frc/robot/utils/FieldUtils.java | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6a39a6b..15b1750 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -561,7 +561,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta == (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue)) .findFirst() - .get())); + .get())); // ---zeroing stuff--- // create triggers for joystick disconnect alerts diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index a5c838a..951a98f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -50,8 +50,8 @@ 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.FeedTargets; import frc.robot.utils.FieldUtils.ClimbTargets; +import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.AutoAlign; diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index 9026dfb..57a4cb0 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -76,8 +76,10 @@ public enum ClimbTargets { 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), + // 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); @@ -89,7 +91,7 @@ private ClimbTargets(Pose2d pose, boolean leftHanded, boolean isBlueAlliance) { this.targetPose = pose; this.leftHanded = leftHanded; this.isBlueAlliance = isBlueAlliance; - } + } public Pose2d getPose() { return targetPose;