diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 0adda27d..caacfe35 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -170,12 +170,16 @@ private void configureDrivebaseBindings() { // applying the request to drive with the inputs s.drivebaseSubsystem .applyRequest( - () -> - drive - .withVelocityX(soloController.isConnected() ? getSoloDriveX() : getDriveX()) - .withVelocityY(soloController.isConnected() ? getSoloDriveY() : getDriveY()) - .withRotationalRate( - soloController.isConnected() ? getSoloDriveRotate() : getDriveRotate())) + () -> { + boolean soloConnected = soloController.isConnected(); + SwerveRequest.FieldCentric request = + drive + .withVelocityX(soloConnected ? getSoloDriveX() : getDriveX()) + .withVelocityY(soloConnected ? getSoloDriveY() : getDriveY()) + .withRotationalRate( + soloConnected ? getSoloDriveRotate() : getDriveRotate()); + return request; + }) .withName("Drive")); // various former controls that were previously used and could be referenced in the future @@ -495,9 +499,12 @@ private Command getCoralBranchHeightCommand(ScoringType version) { } else { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper()); - case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper()); - case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper()); - case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper()); + case CORAL_LEVEL_THREE -> superStructure.coralLevelThree( + driverController.rightBumper(), () -> false); + case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo( + driverController.rightBumper(), () -> false); + case CORAL_LEVEL_ONE -> superStructure.coralLevelOne( + driverController.rightBumper(), () -> false); }; } } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 41ec29d6..6ad54d1d 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -14,6 +14,32 @@ import java.util.function.Supplier; public class SuperStructure { + private static class CoralLevelConfig { + final String name; + final double elevatorPrePos; + final double armScore; + final double preTimeout; + + CoralLevelConfig(String name, double elevatorPrePos, double armScore, double preTimeout) { + this.name = name; + this.elevatorPrePos = elevatorPrePos; + this.armScore = armScore; + this.preTimeout = preTimeout; + } + } + + private static final CoralLevelConfig L4 = + new CoralLevelConfig( + "L4", ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS, ArmPivot.CORAL_PRESET_PRE_L4, 0.7); + + private static final CoralLevelConfig L3 = + new CoralLevelConfig( + "L3", ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS, ArmPivot.CORAL_PRESET_L3, 0.5); + + private static final CoralLevelConfig L2 = + new CoralLevelConfig( + "L2", ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS, ArmPivot.CORAL_PRESET_L2, 0.5); + private final ElevatorSubsystem elevator; private final ArmPivot armPivot; private final SpinnyClaw spinnyClaw; @@ -57,16 +83,6 @@ private Command colorSet(int r, int g, int b, String name) { return elevatorLight.colorSet(r, g, b, name); } - private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) { - // repeats scoring sequence if the coral is still in the claw - if (armSensor == null) { - return Commands.sequence( - command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score)); - } else { - return command.repeatedly().onlyWhile(armSensor.inClaw()); - } - } - private Command repeatPrescoreScoreSwing( Command command, BooleanSupplier score, BooleanSupplier ipScore) { // repeats scoring sequence if the coral is still in the claw @@ -75,133 +91,35 @@ private Command repeatPrescoreScoreSwing( command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(ipScore).until(score)); - } else { - return command.repeatedly().onlyWhile(armSensor.inClaw()); } + return command.repeatedly().onlyWhile(armSensor.inClaw()); } - public Command coralLevelFour(BooleanSupplier score) { - if (branchSensors != null) { // checks if sensor enabled then use for faster scoring + private Command coralLevel( + CoralLevelConfig cfg, + BooleanSupplier score, + BooleanSupplier ipScore, + boolean skipPreIntakeInAuto) { + + if (branchSensors != null) { score = branchSensors.withinScoreRange().or(score); } - return Commands.sequence( - Commands.parallel( - Commands.print("Pre position"), - elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS) - .deadlineFor( // keeps spinny claw engaged until coral has been scored - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), - spinnyClaw.stop()) - .withTimeout(0.7), - repeatPrescoreScoreSwing( - Commands.sequence( - Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_PRE_L4)) - .withDeadline( - Commands.waitUntil( - score)), // waits until driver presses the score button or until - // auto scoring happens - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) - .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), - score), - Commands.print("Pre preIntake()"), - coralPreIntake() - .unless( - RobotModeTriggers - .autonomous()), // doesn't go to preintake if auto, should add for otehr - // commands - Commands.print("Post preIntake()")) - .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L4").asProxy()) - .withName("Coral Level 4"); - } - public Command coralLevelThree(BooleanSupplier score) { // same as L4 return Commands.sequence( Commands.parallel( elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) - .deadlineFor( - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), - spinnyClaw.stop()) - .withTimeout(0.5), - repeatPrescoreScoreSwing( - Commands.repeatingSequence( - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L3) - .withDeadline(Commands.waitUntil(score)), - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) - .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), - score), - coralPreIntake()) - .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) - .withName("Coral Level 3"); - } - - public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3 - return Commands.sequence( - Commands.parallel( - elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) - .deadlineFor( - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), - spinnyClaw.stop()) - .withTimeout(0.5), - repeatPrescoreScoreSwing( - Commands.sequence( - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L2) - .withDeadline(Commands.waitUntil(score)), - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) - .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), - score), - coralPreIntake()) - .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) - .withName("Coral Level 2"); - } - - public Command coralLevelOne(BooleanSupplier score) { - return Commands.sequence( - Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), - spinnyClaw.stop()) // holds coral without wearing flywheels - .withTimeout(0.5) - .withDeadline(Commands.waitUntil(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral - Commands.waitSeconds(1), // Wait to clear the reef - coralPreIntake()) - .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) - .withName("Coral Level 1"); - } - - // New versions with ipScore - // if robot is in position for intermediate scoring, it can score early but if vision is dead - // manual control still works - // only used on solo controls - - public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 - return Commands.sequence( - Commands.parallel( - elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .setLevel(cfg.elevatorPrePos) .deadlineFor( armPivot .moveToPosition(ArmPivot.CORAL_PRESET_UP) .until(ipScore) .until(score)), spinnyClaw.stop()) - .withTimeout(0.5), + .withTimeout(cfg.preTimeout), repeatPrescoreScoreSwing( - Commands.repeatingSequence( + Commands.sequence( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .moveToPosition(cfg.armScore) .withDeadline(Commands.waitUntil(ipScore).until(score)), armPivot .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) @@ -209,38 +127,22 @@ public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), score, ipScore), - coralPreIntake()) - .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) - .withName("Coral Level 3"); + coralPreIntake() + .unless(() -> skipPreIntakeInAuto && RobotModeTriggers.autonomous().getAsBoolean())) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L" + cfg.name).asProxy()) + .withName("Coral Level " + cfg.name); } - public Command coralLevelTwo( - BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3 - return Commands.sequence( - Commands.parallel( - elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) - .deadlineFor( - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_UP) - .until(ipScore) - .until(score)), - spinnyClaw.stop()) - .withTimeout(0.5), - repeatPrescoreScoreSwing( - Commands.sequence( - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L2) - .withDeadline(Commands.waitUntil(ipScore).until(score)), - armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) - .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), - score, - ipScore), - coralPreIntake()) - .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) - .withName("Coral Level 2"); + public Command coralLevelFour(BooleanSupplier score) { + return coralLevel(L4, score, () -> false, true); + } + + public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { + return coralLevel(L3, score, ipScore, false); + } + + public Command coralLevelTwo(BooleanSupplier score, BooleanSupplier ipScore) { + return coralLevel(L2, score, ipScore, false); } public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 93df784a..7d5cc0c8 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -28,8 +28,8 @@ public enum AlignType { } public static Command autoAlign( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls, AlignType type) { - return new AutoAlignCommand(drivebaseSubsystem, controls, type).withName("Auto Align"); + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls, AlignType alignType) { + return new AutoAlignCommand(drivebaseSubsystem, controls, alignType).withName("Auto Align"); } public static boolean readyToScore() { @@ -49,14 +49,14 @@ public static boolean isLevel() { && MathUtil.isNear(0, rotation.getY(), Units.degreesToRadians(2)); } - public static boolean isCloseEnough(AlignType type) { + public static boolean isCloseEnough(AlignType alignType) { var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; - var branchPose = AutoAlignCommand.getTargetPose(currentPose, type); + var branchPose = AutoAlignCommand.getTargetPose(currentPose, alignType); return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } - public static boolean poseInPlace(AlignType type) { - return isStationary() && isCloseEnough(type); + public static boolean poseInPlace(AlignType alignType) { + return isStationary() && isCloseEnough(alignType); } public static boolean @@ -182,6 +182,75 @@ public static boolean poseInPlace(AlignType type) { private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); + private static final List allBlueBranches = + List.of( + blueBranchA, + blueBranchB, + blueBranchC, + blueBranchD, + blueBranchE, + blueBranchF, + blueBranchG, + blueBranchH, + blueBranchI, + blueBranchJ, + blueBranchK, + blueBranchL); + private static final List allRedBranches = + List.of( + redBranchA, + redBranchB, + redBranchC, + redBranchD, + redBranchE, + redBranchF, + redBranchG, + redBranchH, + redBranchI, + redBranchJ, + redBranchK, + redBranchL); + private static final List leftBlueBranches = + List.of(blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK); + private static final List leftRedBranches = + List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); + private static final List rightBlueBranches = + List.of(blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL); + private static final List rightRedBranches = + List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); + private static final List leftL1BluePoses = + List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL); + private static final List leftL1RedPoses = + List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); + private static final List rightL1BluePoses = + List.of( + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL); + private static final List rightL1RedPoses = + List.of( + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); + private static class AutoAlignCommand extends Command { protected final PIDController pidX = new PIDController(4, 0, 0); @@ -191,25 +260,25 @@ private static class AutoAlignCommand extends Command { protected final CommandSwerveDrivetrain drive; protected final Controls controls; protected Pose2d branchPose; - protected AlignType type; + protected AlignType alignType; private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric() // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls, AlignType type) { + public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls, AlignType alignType) { this.drive = drive; pidRotate.enableContinuousInput(-Math.PI, Math.PI); this.controls = controls; - this.type = type; + this.alignType = alignType; setName("Auto Align"); } @Override public void initialize() { Pose2d robotPose = drive.getState().Pose; - branchPose = getTargetPose(robotPose, type); + branchPose = getTargetPose(robotPose, alignType); pidX.setSetpoint(branchPose.getX()); pidY.setSetpoint(branchPose.getY()); pidRotate.setSetpoint(branchPose.getRotation().getRadians()); @@ -254,104 +323,19 @@ public void end(boolean interrupted) { controls.vibrateDriveController(0); } - public static Pose2d getTargetPose(Pose2d pose, AlignType type) { - return switch (type) { - case LEFTB -> getNearestLeftBranch(pose); - case RIGHTB -> getNearestRightBranch(pose); - case L1LB -> getNearestL1L(pose); - case L1RB -> getNearestL1R(pose); - case ALLB -> getNearestBranch(pose); + public static Pose2d getTargetPose(Pose2d pose, AlignType alignType) { + return switch (alignType) { + case LEFTB -> getNearestBranch(pose, leftBlueBranches, leftRedBranches); + case RIGHTB -> getNearestBranch(pose, rightBlueBranches, rightRedBranches); + case L1LB -> getNearestBranch(pose, leftL1BluePoses, leftL1RedPoses); + case L1RB -> getNearestBranch(pose, rightL1BluePoses, rightL1RedPoses); + case ALLB -> getNearestBranch(pose, allBlueBranches, allRedBranches); }; } - private static Pose2d getNearestBranch(Pose2d p) { - List branchPose2ds = - AllianceUtils.isBlue() - ? List.of( - blueBranchA, - blueBranchB, - blueBranchC, - blueBranchD, - blueBranchE, - blueBranchF, - blueBranchG, - blueBranchH, - blueBranchI, - blueBranchJ, - blueBranchK, - blueBranchL) - : List.of( - redBranchA, - redBranchB, - redBranchC, - redBranchD, - redBranchE, - redBranchF, - redBranchG, - redBranchH, - redBranchI, - redBranchJ, - redBranchK, - redBranchL); - return p.nearest(branchPose2ds); - } - - private static Pose2d getNearestLeftBranch(Pose2d p) { - List branchPose2ds = - AllianceUtils.isBlue() - ? List.of( - blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK) - : List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); + private static Pose2d getNearestBranch(Pose2d p, List bluePoses, List redPoses) { + List branchPose2ds = AllianceUtils.isBlue() ? bluePoses : redPoses; return p.nearest(branchPose2ds); } - - private static Pose2d getNearestRightBranch(Pose2d p) { - List branchPose2ds = - AllianceUtils.isBlue() - ? List.of( - blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL) - : List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); - return p.nearest(branchPose2ds); - } - - private static Pose2d getNearestL1L(Pose2d p) { - List reefFacesPose2ds = - AllianceUtils.isBlue() - ? List.of( - lBlueReefFaceAB, - lBlueReefFaceCD, - lBlueReefFaceEF, - lBlueReefFaceGH, - lBlueReefFaceIJ, - lBlueReefFaceKL) - : List.of( - lRedReefFaceAB, - lRedReefFaceCD, - lRedReefFaceEF, - lRedReefFaceGH, - lRedReefFaceIJ, - lRedReefFaceKL); - return p.nearest(reefFacesPose2ds); - } - - private static Pose2d getNearestL1R(Pose2d p) { - List reefFacesPose2ds = - AllianceUtils.isBlue() - ? List.of( - rBlueReefFaceAB, - rBlueReefFaceCD, - rBlueReefFaceEF, - rBlueReefFaceGH, - rBlueReefFaceIJ, - rBlueReefFaceKL) - : List.of( - rRedReefFaceAB, - rRedReefFaceCD, - rRedReefFaceEF, - rRedReefFaceGH, - rRedReefFaceIJ, - rRedReefFaceKL); - return p.nearest(reefFacesPose2ds); - } } }