diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dbedcf8..15b1750 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; @@ -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() { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index e689ae0..951a98f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/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 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 e9d49d0..57a4cb0 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/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 CLIMB_TARGETS_LIST = + Arrays.stream(ClimbTargets.values()).toList(); + } }