From 7a86d58709aa6b52cf3a7a3450cab8d3dab93bde Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 08:56:02 -0700 Subject: [PATCH 01/24] elastic camera fix --- src/main/deploy/elastic-layout.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 54ccf5f3..7e3a60b3 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -100,7 +100,7 @@ "height": 512.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/left", + "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", "period": 0.06, "rotation_turns": 0 } @@ -113,7 +113,7 @@ "height": 512.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/right", + "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", "period": 0.06, "rotation_turns": 0 } From c8fcedc0e2d4372629db33a51b9d79a6c620dda7 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 09:12:29 -0700 Subject: [PATCH 02/24] autoScore stuff --- src/main/java/frc/robot/Controls.java | 6 +- .../frc/robot/subsystems/SuperStructure.java | 83 +++++++++++++++++++ 2 files changed, 86 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 2ba35c91..8eb77a0c 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -463,13 +463,13 @@ private Command getSoloCoralBranchHeightCommand() { .coralLevelFour(soloController.rightBumper()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper()) + .coralLevelThree(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper()) + .coralLevelTwo(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper()) + .coralLevelOne(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..d7dc5092 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -67,6 +67,17 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) } } + private Command repeatPrescoreScoreSwing( + Command command, BooleanSupplier score, BooleanSupplier ipScore) { + // 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()); + } + } + public Command coralLevelFour(BooleanSupplier score) { if (branchSensors != null) { // checks if sensor enabled then use for faster scoring score = branchSensors.withinScoreRange().or(score); @@ -168,6 +179,78 @@ public Command coralLevelOne(BooleanSupplier score) { .withName("Coral Level 1"); } + public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.repeatingSequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .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 L3").asProxy()) + .withName("Coral Level 3"); + } + + 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 coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { + 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(ipScore).until(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"); + } + // quickGroundIntake() is used instead since it's faster and still reliable. // (This one moves the coral intake the normal intake position, then does the normal intake. // quickGroundIntake() instead hands the coral directly to the claw.) From 1014c22fbde2c2ec2f1d11347244cffbaa5ef34d Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 09:51:18 -0700 Subject: [PATCH 03/24] l1 auto align --- src/main/java/frc/robot/Controls.java | 10 ++++ .../frc/robot/subsystems/auto/AutoAlign.java | 55 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 8eb77a0c..51a8a39c 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -969,6 +969,11 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -1006,6 +1011,11 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); // Ground Intake soloController .leftBumper() diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 0f0ad2d4..757e6287 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,6 +23,10 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } + public static Command autoAlignL1(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1(drivebaseSubsystem, controls).withName("Auto Align"); + } + public static Command autoAlignLeft( CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandLeft(drivebaseSubsystem, controls).withName("Auto Align"); @@ -138,6 +142,31 @@ public static boolean isCloseEnough() { private static final Pose2d redBranchL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d(); + private static final Pose2d blueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d(); + private static final Pose2d blueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d(); + private static final Pose2d blueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d(); + private static final Pose2d blueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d(); + private static final Pose2d blueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d(); + + private static final Pose2d redReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d(); + private static final Pose2d redReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d(); + private static final Pose2d redReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d(); + private static final Pose2d redReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d(); + private static final Pose2d redReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d(); + private static final Pose2d redReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d(); private static final List blueBranchPoses = List.of( blueBranchA, @@ -294,4 +323,30 @@ public void initialize() { pidRotate.setSetpoint(branchPose.getRotation().getRadians()); } } + + private static class AutoAlignCommandL1 extends AutoAlignCommand { + private static final List blueReefFaces = + List.of(blueReefFaceAB, blueReefFaceCD, blueReefFaceEF, blueReefFaceGH, blueReefFaceIJ, blueReefFaceKL); + + private static final List redReefFaces = + List.of(redReefFaceAB, redReefFaceCD, redReefFaceEF, redReefFaceGH, redReefFaceIJ, redReefFaceKL); + + public static Pose2d getNearestReefFace(Pose2d p) { + List branchPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(branchPose2ds); + } + + public AutoAlignCommandL1(CommandSwerveDrivetrain drive, Controls controls) { + super(drive, controls); + } + + @Override + public void initialize() { + Pose2d robotPose = drive.getState().Pose; + branchPose = getNearestReefFace(robotPose); + pidX.setSetpoint(branchPose.getX()); + pidY.setSetpoint(branchPose.getY()); + pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + } + } } From 0d2405ebd833eb64d03ca512cf682ec13a6d246c Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 09:53:49 -0700 Subject: [PATCH 04/24] ee --- src/main/java/frc/robot/Controls.java | 12 ++++++------ .../java/frc/robot/subsystems/auto/AutoAlign.java | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 8eb77a0c..3de11c8d 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -463,13 +463,13 @@ private Command getSoloCoralBranchHeightCommand() { .coralLevelFour(soloController.rightBumper()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), ()-> AutoAlign.poseInPlace()) + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } @@ -1095,11 +1095,11 @@ private void configureSoloControllerBindings() { .startMovingVoltage( () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) .withName("Elevator Manual Control")); - // Ready to score rumble - Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace()); + // Ready to score rumble (Removal for autoscoring) + /* Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace()); readyToScore.onTrue( Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0))); readyToScore.onFalse( - Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); + Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); */ } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 0f0ad2d4..35758bf7 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -83,10 +83,10 @@ public static boolean isCloseEnough() { // left and right offsets from the april tags () private static final Transform2d leftOfTag = new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(36 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); private static final Transform2d rightOfTag = new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(36 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); From e00f8279b09a2908d8e709852aa728a650763ae8 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 10:12:22 -0700 Subject: [PATCH 05/24] c --- src/main/java/frc/robot/Controls.java | 2 +- .../frc/robot/subsystems/SuperStructure.java | 2 +- .../frc/robot/subsystems/auto/AutoAlign.java | 50 ++++++++++++------- 3 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 6330f955..f3c2b3ef 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -970,7 +970,7 @@ private void configureSoloControllerBindings() { .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); soloController - .rightTrigger() + .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d7dc5092..0e4e28ea 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -244,7 +244,7 @@ public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral + spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 19f16e5f..b76bc5e6 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -87,10 +87,12 @@ public static boolean isCloseEnough() { // left and right offsets from the april tags () private static final Transform2d leftOfTag = new Transform2d( - Units.inchesToMeters(36 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(34 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); private static final Transform2d rightOfTag = new Transform2d( - Units.inchesToMeters(36 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + private static final Transform2d middleOfReef = + new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -143,30 +145,30 @@ public static boolean isCloseEnough() { aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); private static final Pose2d blueReefFaceAB = - aprilTagFieldLayout.getTagPose(18).get().toPose2d(); + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceCD = - aprilTagFieldLayout.getTagPose(17).get().toPose2d(); + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceEF = - aprilTagFieldLayout.getTagPose(22).get().toPose2d(); + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceGH = - aprilTagFieldLayout.getTagPose(21).get().toPose2d(); + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceIJ = - aprilTagFieldLayout.getTagPose(20).get().toPose2d(); + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(middleOfReef); private static final Pose2d blueReefFaceKL = - aprilTagFieldLayout.getTagPose(19).get().toPose2d(); + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceAB = - aprilTagFieldLayout.getTagPose(7).get().toPose2d(); + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceCD = - aprilTagFieldLayout.getTagPose(8).get().toPose2d(); + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceEF = - aprilTagFieldLayout.getTagPose(9).get().toPose2d(); + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceGH = - aprilTagFieldLayout.getTagPose(10).get().toPose2d(); + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceIJ = - aprilTagFieldLayout.getTagPose(11).get().toPose2d(); + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(middleOfReef); private static final Pose2d redReefFaceKL = - aprilTagFieldLayout.getTagPose(6).get().toPose2d(); + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(middleOfReef); private static final List blueBranchPoses = List.of( blueBranchA, @@ -326,14 +328,26 @@ public void initialize() { private static class AutoAlignCommandL1 extends AutoAlignCommand { private static final List blueReefFaces = - List.of(blueReefFaceAB, blueReefFaceCD, blueReefFaceEF, blueReefFaceGH, blueReefFaceIJ, blueReefFaceKL); + List.of( + blueReefFaceAB, + blueReefFaceCD, + blueReefFaceEF, + blueReefFaceGH, + blueReefFaceIJ, + blueReefFaceKL); private static final List redReefFaces = - List.of(redReefFaceAB, redReefFaceCD, redReefFaceEF, redReefFaceGH, redReefFaceIJ, redReefFaceKL); + List.of( + redReefFaceAB, + redReefFaceCD, + redReefFaceEF, + redReefFaceGH, + redReefFaceIJ, + redReefFaceKL); public static Pose2d getNearestReefFace(Pose2d p) { - List branchPose2ds = isBlue() ? blueReefFaces : redReefFaces; - return p.nearest(branchPose2ds); + List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(reefFacesPose2ds); } public AutoAlignCommandL1(CommandSwerveDrivetrain drive, Controls controls) { From 088c91719e95f31fca68d9498bd860d081b812b8 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 10:37:42 -0700 Subject: [PATCH 06/24] super jank code to get auto l1 auto extake --- src/main/java/frc/robot/Controls.java | 29 +++- .../frc/robot/subsystems/auto/AutoAlign.java | 146 ++++++++++++++---- 2 files changed, 141 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index f3c2b3ef..a616dda5 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -457,7 +457,7 @@ private Command getCoralBranchHeightCommand() { }; } - private Command getSoloCoralBranchHeightCommand() { + private Command getSoloCoralBranchHeightCommandL() { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) @@ -469,7 +469,24 @@ private Command getSoloCoralBranchHeightCommand() { .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } + + private Command getSoloCoralBranchHeightCommandR() { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } @@ -926,7 +943,7 @@ private void configureSoloControllerBindings() { switch (soloScoringMode) { case CORAL_IN_CLAW -> { scoreCommand = - getSoloCoralBranchHeightCommand() + getSoloCoralBranchHeightCommandL() .until( () -> soloController.a().getAsBoolean() @@ -973,7 +990,7 @@ private void configureSoloControllerBindings() { .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlignL1L(s.drivebaseSubsystem, this)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -982,7 +999,7 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand() + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommandR() .until( () -> soloController.a().getAsBoolean() @@ -1015,7 +1032,7 @@ private void configureSoloControllerBindings() { .rightTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignL1(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlignL1R(s.drivebaseSubsystem, this)); // Ground Intake soloController .leftBumper() diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index b76bc5e6..43fe7cf6 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,8 +23,12 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandL1(drivebaseSubsystem, controls).withName("Auto Align"); + public static Command autoAlignL1L(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); + } + + public static Command autoAlignL1R(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); } public static Command autoAlignLeft( @@ -56,6 +60,14 @@ public static boolean poseInPlace() { return isStationary() && isCloseEnough(); } + public static boolean poseInPlaceL1L() { + return isStationary() && isCloseEnoughL1L(); + } + + public static boolean poseInPlaceL1R() { + return isStationary() && isCloseEnoughL1R(); + } + public static boolean isStationary() { var speeds = AutoLogic.s.drivebaseSubsystem.getState().Speeds; return MathUtil.isNear(0, speeds.vxMetersPerSecond, 0.01) @@ -75,6 +87,18 @@ public static boolean isCloseEnough() { return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } + public static boolean isCloseEnoughL1L() { + var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var branchPose = AutoAlignCommandL1L.getNearestReefFace(currentPose); + return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + } + + public static boolean isCloseEnoughL1R() { + var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var branchPose = AutoAlignCommandL1R.getNearestReefFace(currentPose); + return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + } + public static boolean oneSecondLeft() { // THIS WILL ONLY WORK ON THE REAL FIELD AND IN PRACTICE MODE! @@ -93,6 +117,7 @@ public static boolean isCloseEnough() { Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Transform2d middleOfReef = new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = new Transform2d(Units.inchesToMeters(34/2), Units.inchesToMeters(13.5/2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -144,31 +169,58 @@ public static boolean isCloseEnough() { private static final Pose2d redBranchL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); - private static final Pose2d blueReefFaceAB = + private static final Pose2d lBlueReefFaceAB = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceCD = + private static final Pose2d lBlueReefFaceCD = aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceEF = + private static final Pose2d lBlueReefFaceEF = aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceGH = + private static final Pose2d lBlueReefFaceGH = aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceIJ = + private static final Pose2d lBlueReefFaceIJ = aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(middleOfReef); - private static final Pose2d blueReefFaceKL = + private static final Pose2d lBlueReefFaceKL = aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceAB = + private static final Pose2d rBlueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(l1RightOfReef); + + private static final Pose2d lRedReefFaceAB = aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceCD = + private static final Pose2d lRedReefFaceCD = aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceEF = + private static final Pose2d lRedReefFaceEF = aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceGH = + private static final Pose2d lRedReefFaceGH = aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceIJ = + private static final Pose2d lRedReefFaceIJ = aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(middleOfReef); - private static final Pose2d redReefFaceKL = + private static final Pose2d lRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(middleOfReef); + + private static final Pose2d rRedReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); + private static final List blueBranchPoses = List.of( blueBranchA, @@ -326,31 +378,69 @@ public void initialize() { } } - private static class AutoAlignCommandL1 extends AutoAlignCommand { + private static class AutoAlignCommandL1L extends AutoAlignCommand { + private static final List blueReefFaces = + List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL); + + private static final List redReefFaces = + List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); + + public static Pose2d getNearestReefFace(Pose2d p) { + List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(reefFacesPose2ds); + } + + public AutoAlignCommandL1L(CommandSwerveDrivetrain drive, Controls controls) { + super(drive, controls); + } + + @Override + public void initialize() { + Pose2d robotPose = drive.getState().Pose; + branchPose = getNearestReefFace(robotPose); + pidX.setSetpoint(branchPose.getX()); + pidY.setSetpoint(branchPose.getY()); + pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + } + } + + private static class AutoAlignCommandL1R extends AutoAlignCommand { private static final List blueReefFaces = List.of( - blueReefFaceAB, - blueReefFaceCD, - blueReefFaceEF, - blueReefFaceGH, - blueReefFaceIJ, - blueReefFaceKL); + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL); private static final List redReefFaces = List.of( - redReefFaceAB, - redReefFaceCD, - redReefFaceEF, - redReefFaceGH, - redReefFaceIJ, - redReefFaceKL); + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); public static Pose2d getNearestReefFace(Pose2d p) { List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; return p.nearest(reefFacesPose2ds); } - public AutoAlignCommandL1(CommandSwerveDrivetrain drive, Controls controls) { + public AutoAlignCommandL1R(CommandSwerveDrivetrain drive, Controls controls) { super(drive, controls); } From e1b832af1657e7c9197c20291518ed1e1f37a492 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 18 Oct 2025 15:15:04 -0700 Subject: [PATCH 07/24] c --- src/main/java/frc/robot/Controls.java | 9 ++------- .../java/frc/robot/subsystems/SuperStructure.java | 4 +++- .../java/frc/robot/subsystems/auto/AutoAlign.java | 12 ++++++++---- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index a616dda5..b0b3fd96 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -371,7 +371,7 @@ private void configureSuperStructureBindings() { .and(RobotModeTriggers.teleop()) .onTrue( superStructure - .coralIntake() + .coralIntake().alongWith(Commands.runOnce(() -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW)) .alongWith( s.elevatorLEDSubsystem != null ? s.elevatorLEDSubsystem @@ -394,12 +394,7 @@ private void configureSuperStructureBindings() { case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; } }) - .withName("Set solo scoring mode")); - - sensors - .armSensor - .inClaw() - .and(RobotModeTriggers.teleop()) + .withName("Set solo scoring mode")) .onFalse( Commands.runOnce( () -> { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 0e4e28ea..570839d1 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -244,7 +244,9 @@ public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.5 /* this time could be shorter */), // spits out coral + spinnyClaw + .coralLevelOneHoldExtakePower() + .withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 43fe7cf6..902e580b 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,11 +23,13 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1L(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1L( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1R(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1R( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); } @@ -117,7 +119,9 @@ public static boolean isCloseEnoughL1R() { Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Transform2d middleOfReef = new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); - private static final Transform2d l1RightOfReef = new Transform2d(Units.inchesToMeters(34/2), Units.inchesToMeters(13.5/2), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(13.5 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -220,7 +224,7 @@ public static boolean isCloseEnoughL1R() { aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); - + private static final List blueBranchPoses = List.of( blueBranchA, From c1835195e3ea09cd794094ac48a2a190ba916407 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sun, 19 Oct 2025 13:36:06 -0700 Subject: [PATCH 08/24] ds changes --- src/main/deploy/elastic-layout.json | 1170 +++++------------ src/main/java/frc/robot/Controls.java | 8 + .../frc/robot/subsystems/SuperStructure.java | 4 +- .../frc/robot/subsystems/auto/AutoAlign.java | 12 +- 4 files changed, 339 insertions(+), 855 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 7e3a60b3..40397540 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -3,130 +3,33 @@ "grid_size": 128, "tabs": [ { - "name": "Match", + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "ArmSensor", "grid_layout": { "layouts": [], "containers": [ { - "title": "has arm sensor", + "title": "inTrough", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Match/has arm sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has ground intake sensor", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has ground intake sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has left branch sensor", - "x": 0.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has left branch sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has right branch sensor", - "x": 128.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has right branch sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has recent vision measurements", - "x": 384.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has recent vision measurements", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "left cam", - "x": 256.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "right cam", - "x": 768.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "Is Zeroed", - "x": 640.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/Is Zeroed", + "topic": "/Shuffleboard/ArmSensor/inTrough", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -139,465 +42,44 @@ } }, { - "name": "Autos", + "name": "IntakeSensor", "grid_layout": { "layouts": [], "containers": [ { - "title": "Auto Delay", - "x": 640.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Autos/Auto Delay", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Starting Position", - "x": 512.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Starting Position", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Game Objects", - "x": 768.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Game Objects", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Available Auto Variants", - "x": 512.0, - "y": 128.0, - "width": 512.0, - "height": 256.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Available Auto Variants", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Close Enough?", - "x": 1152.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/Close Enough?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Level?", - "x": 1280.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/Level?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Stationary?", - "x": 1408.0, + "title": "In Intake", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Autos/Stationary?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "readyToScore?", - "x": 1152.0, - "y": 128.0, - "width": 384.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/readyToScore?", + "topic": "/Shuffleboard/IntakeSensor/In Intake", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Selected auto", - "x": 0.0, - "y": 256.0, - "width": 512.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Shuffleboard/Field/Selected auto", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "Start pose", - "x": 1024.0, - "y": 256.0, - "width": 512.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Shuffleboard/Field/Start pose", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "Auto display speed", - "x": 0.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Shuffleboard/Field/Auto display speed", - "period": 0.06, - "data_type": "double", - "min_value": -0.5, - "max_value": 3.0, - "divisions": 5, - "update_continuously": false - } - } - ] - } - }, - { - "name": "AprilTags", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Last raw timestamp", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Last raw timestamp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Num targets", - "x": 0.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Num targets", - "period": 0.06, - "data_type": "int", - "show_submit_button": false - } - }, - { - "title": "Last timestamp", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Last timestamp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "april tag distance meters", - "x": 128.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/april tag distance meters", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "time since last reading", - "x": 256.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/time since last reading", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Disable vision", - "x": 512.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Toggle Button", - "properties": { - "topic": "/Shuffleboard/AprilTags/Disable vision", - "period": 0.06, - "data_type": "boolean" - } - } - ] - } - }, - { - "name": "Elevator", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Elevator Speed", - "x": 640.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/Elevator Speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Is Zeroed", - "x": 512.0, - "y": 128.0, - "width": 512.0, - "height": 512.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/Is Zeroed", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 at forward softstop", - "x": 0.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 at forward softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 at reverse softstop", - "x": 0.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 at reverse softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 output voltage", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 output voltage", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M1 supply current", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 supply current", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 at forward softstop", - "x": 1280.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 at forward softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M2 at reverse softstop", - "x": 1280.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 at reverse softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M2 output voltage", - "x": 1408.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 output voltage", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 supply current", - "x": 1280.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 supply current", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 temp", - "x": 1152.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 temp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false + "false_icon": "None" } - }, + } + ] + } + }, + { + "name": "BranchSensor", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "M1 temp", - "x": 256.0, + "title": "LeftDistance(m)", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Elevator/M1 temp", + "topic": "/Shuffleboard/BranchSensor/LeftDistance(m)", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -607,179 +89,132 @@ } }, { - "name": "End Effector", + "name": "AprilTags", "grid_layout": { "layouts": [], "containers": [ { - "title": "Pivot Target Pos", - "x": 512.0, + "title": "Last raw timestamp", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Target Pos", + "topic": "/Shuffleboard/AprilTags/Last raw timestamp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Speed", - "x": 256.0, - "y": 0.0, + "title": "Num targets", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "topic": "/Shuffleboard/AprilTags/Num targets", "period": 0.06, - "data_type": "double", + "data_type": "int", "show_submit_button": false } }, { - "title": "Pivot Position", + "title": "Last timestamp", "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Position", + "topic": "/Shuffleboard/AprilTags/Last timestamp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Motor rotor Pos", - "x": 0.0, - "y": 0.0, + "title": "april tag distance meters", + "x": 128.0, + "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Motor rotor Pos", + "topic": "/Shuffleboard/AprilTags/april tag distance meters", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Motor Temperature", - "x": 384.0, + "title": "time since last reading", + "x": 256.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Motor Temperature", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Distance(m)", - "x": 640.0, - "y": 512.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/ArmSensor/Distance(m)", + "topic": "/Shuffleboard/AprilTags/time since last reading", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "inClaw", + "title": "Disable vision", "x": 512.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/ArmSensor/inClaw", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "inTrough", - "x": 768.0, - "y": 256.0, - "width": 256.0, + "y": 0.0, + "width": 384.0, "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/ArmSensor/inTrough", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Claw Motor Temperature", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Claw/Claw Motor Temperature", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Claw Speed", - "x": 1024.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", + "type": "Toggle Button", "properties": { - "topic": "/Shuffleboard/Claw/Claw Speed", + "topic": "/Shuffleboard/AprilTags/Disable vision", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Elevator", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Last Set Power", - "x": 1152.0, - "y": 128.0, + "title": "Motor Current Position", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Claw/Last Set Power", + "topic": "/Shuffleboard/Elevator/Motor Current Position", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "Arm Pivot", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Motor Voltage", - "x": 1408.0, - "y": 128.0, + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Claw/Motor Voltage", + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -795,9 +230,9 @@ "containers": [ { "title": "Is Climb OUT?", - "x": 640.0, - "y": 128.0, - "width": 256.0, + "x": 0.0, + "y": 0.0, + "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -809,137 +244,68 @@ "true_icon": "None", "false_icon": "None" } - }, + } + ] + } + }, + { + "name": "Claw", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Is Climb STOWED?", - "x": 640.0, + "title": "Claw Speed", + "x": 0.0, "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Is Climb STOWED?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Motor Position", - "x": 1152.0, - "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Motor Position", + "topic": "/Shuffleboard/Claw/Claw Speed", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "GroundIntake", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Motor Speed", - "x": 1152.0, + "title": "Speed", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Motor Speed", + "topic": "/Shuffleboard/GroundIntake/Speed", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, - { - "title": "Move Complete?", - "x": 384.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Move Complete?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, + } + ] + } + }, + { + "name": "Ground Arm", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Set speed", - "x": 1280.0, + "title": "Pivot Speed", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Set speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Where Move next?", - "x": 640.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/Where Move next?", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Where moving?", - "x": 640.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/Where moving?", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Within Tolerance?", - "x": 896.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Within Tolerance?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "targetPos", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/targetPos", + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -949,212 +315,316 @@ } }, { - "name": "GroundIntake", + "name": "Controls", "grid_layout": { "layouts": [], "containers": [ { - "title": "Speed", + "title": "Swerve Coast Mode", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Toggle Button", "properties": { - "topic": "/Shuffleboard/GroundIntake/Speed", + "topic": "/Shuffleboard/Controls/Swerve Coast Mode", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Autos", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Pivot Motor Temperature", + "title": "readyToScore?", "x": 0.0, - "y": 384.0, + "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Motor Temperature", + "topic": "/Shuffleboard/Autos/readyToScore?", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Pivot Motor rotor Pos", - "x": 128.0, - "y": 384.0, - "width": 128.0, + "title": "Starting Position", + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Motor rotor Pos", + "topic": "/Shuffleboard/Autos/Starting Position", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Position", - "x": 256.0, - "y": 384.0, + "title": "Launch Type", + "x": 512.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Position", + "topic": "/Shuffleboard/Autos/Launch Type", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Speed", - "x": 384.0, - "y": 384.0, + "title": "Game Objects", + "x": 640.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Speed", + "topic": "/Shuffleboard/Autos/Game Objects", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Target Pos", + "title": "Available Auto Variants", "x": 512.0, - "y": 384.0, + "y": 256.0, "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Target Pos", + "topic": "/Shuffleboard/Autos/Available Auto Variants", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Last Set Power", - "x": 0.0, - "y": 128.0, + "title": "Auto Delay", + "x": 512.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/GroundIntake/Last Set Power", + "topic": "/Shuffleboard/Autos/Auto Delay", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Motor Temperature", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", + "title": "photonvision_Port_1182_Output_MJPEG_Server", + "x": 256.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", "properties": { - "topic": "/Shuffleboard/GroundIntake/Motor Temperature", + "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "rotation_turns": 0 } }, { - "title": "Motor Voltage", - "x": 128.0, - "y": 128.0, - "width": 128.0, + "title": "photonvision_Port_1186_Output_MJPEG_Server", + "x": 640.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + }, + { + "name": "Field", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Auto display speed", + "x": 1280.0, + "y": 640.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Number Slider", "properties": { - "topic": "/Shuffleboard/GroundIntake/Motor Voltage", + "topic": "/Shuffleboard/Field/Auto display speed", "period": 0.06, "data_type": "double", - "show_submit_button": false + "min_value": -1.0, + "max_value": 1.0, + "divisions": 5, + "update_continuously": false } }, { - "title": "Distance(m)", - "x": 640.0, - "y": 0.0, + "title": "Est. Time (s)", + "x": 1280.0, + "y": 512.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/IntakeSensor/Distance(m)", + "topic": "/Shuffleboard/Field/Est. Time (s)", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "In Intake", - "x": 768.0, + "title": "Selected auto", + "x": 0.0, "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", + "width": 1280.0, + "height": 768.0, + "type": "Field", "properties": { - "topic": "/Shuffleboard/IntakeSensor/In Intake", + "topic": "/Shuffleboard/Field/Selected auto", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 } } ] } }, { - "name": "Controls", + "name": "Match", "grid_layout": { "layouts": [], "containers": [ { - "title": "Elevator Coast Mode", - "x": 384.0, + "title": "has arm sensor", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Elevator Coast Mode", + "topic": "/Shuffleboard/Match/has arm sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Swerve Coast Mode", - "x": 512.0, + "title": "has ground intake sensor", + "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Swerve Coast Mode", + "topic": "/Shuffleboard/Match/has ground intake sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Climb Coast Mode", - "x": 640.0, - "y": 0.0, + "title": "has left branch sensor", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has left branch sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has right branch sensor", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Climb Coast Mode", + "topic": "/Shuffleboard/Match/has right branch sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has recent vision measurements", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has recent vision measurements", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "left cam", + "x": 256.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/left", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "right cam", + "x": 768.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/right", + "period": 0.06, + "rotation_turns": 0 } } ] diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index a616dda5..7ec94515 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -112,6 +112,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureGroundSpinnyBindings(); configureGroundArmBindings(); configureSoloControllerBindings(); + Shuffleboard.getTab("Elevator") + .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE); + // Shuffleboard.getTab("Elevator") + // .addString("Scoring Mode", () -> getSoloScoringMode().toString()); + } + + public SoloScoringMode getSoloScoringMode() { + return soloScoringMode; } private Trigger connected(CommandXboxController controller) { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 0e4e28ea..570839d1 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -244,7 +244,9 @@ public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), - spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.5 /* this time could be shorter */), // spits out coral + spinnyClaw + .coralLevelOneHoldExtakePower() + .withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 43fe7cf6..726558d8 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,11 +23,13 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1L(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1L( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); } - public static Command autoAlignL1R(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + public static Command autoAlignL1R( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); } @@ -117,7 +119,9 @@ public static boolean isCloseEnoughL1R() { Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); private static final Transform2d middleOfReef = new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); - private static final Transform2d l1RightOfReef = new Transform2d(Units.inchesToMeters(34/2), Units.inchesToMeters(13.5/2), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(26 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -220,7 +224,7 @@ public static boolean isCloseEnoughL1R() { aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); - + private static final List blueBranchPoses = List.of( blueBranchA, From 0bcbd20a956cb096e48deeaf1470a234b0520dc2 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sun, 19 Oct 2025 13:36:55 -0700 Subject: [PATCH 09/24] quick hand off fix --- src/main/java/frc/robot/Controls.java | 2 +- src/main/java/frc/robot/subsystems/SuperStructure.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b0b3fd96..7267be6b 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -371,7 +371,7 @@ private void configureSuperStructureBindings() { .and(RobotModeTriggers.teleop()) .onTrue( superStructure - .coralIntake().alongWith(Commands.runOnce(() -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW)) + .coralIntake() .alongWith( s.elevatorLEDSubsystem != null ? s.elevatorLEDSubsystem diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 570839d1..b1b5624e 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -301,11 +301,11 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Make the big sequence. return Commands.sequence( // Initial setup- Move elevator high enough for ground arm to be clear, start moving - // arm pivot, stop the spinny claw, and start spinning the ground intake + // arm pivot, and start spinning the ground intake Commands.parallel( elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), - spinnyClaw.stop(), // just as a backup in case things are silly + spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), From b2d668730f7a954b966a611709acb575b9518cac Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 00:41:48 -0700 Subject: [PATCH 10/24] log + spot --- src/main/java/frc/robot/Controls.java | 4 ++-- src/main/java/frc/robot/subsystems/SuperStructure.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 6cd226dc..36eb85d2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -114,8 +114,8 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureSoloControllerBindings(); Shuffleboard.getTab("Elevator") .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE); - // Shuffleboard.getTab("Elevator") - // .addString("Scoring Mode", () -> getSoloScoringMode().toString()); + Shuffleboard.getTab("Elevator") + .addString("Scoring Mode", () -> getSoloScoringMode().toString()); } public SoloScoringMode getSoloScoringMode() { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index b1b5624e..217675a6 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -305,7 +305,7 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph Commands.parallel( elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), - spinnyClaw.coralIntakePower(), + spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), From 7fc3da121268c3397f6689dd73f00e93d9e58196 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 23:07:06 -0700 Subject: [PATCH 11/24] Code Clean up for get coral branch height --- src/main/java/frc/robot/Controls.java | 88 +++++++++++++-------------- 1 file changed, 43 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 36eb85d2..5c069acc 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -295,7 +295,7 @@ private void configureSuperStructureBindings() { Commands.deferredProxy( () -> switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getCoralBranchHeightCommand("2C"); case ALGAE -> Commands.sequence( superStructure.algaeProcessorScore( driverController.rightBumper()), @@ -426,7 +426,7 @@ private void configureSuperStructureBindings() { () -> { Command scoreCommand = switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getCoralBranchHeightCommand("2C"); case ALGAE -> Commands.sequence( BargeAlign.bargeScore( s.drivebaseSubsystem, @@ -451,47 +451,45 @@ private Command getAlgaeIntakeCommand() { }; } - private Command getCoralBranchHeightCommand() { - 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()); - }; - } - - private Command getSoloCoralBranchHeightCommandL() { - return switch (branchHeight) { - case CORAL_LEVEL_FOUR -> superStructure - .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - }; - } - - private Command getSoloCoralBranchHeightCommandR() { - return switch (branchHeight) { - case CORAL_LEVEL_FOUR -> superStructure - .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - }; + private Command getCoralBranchHeightCommand(String version) { + if (version.equals("SL")) { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } else if (version.equals("SR")) { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } 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()); + }; + } } private void configureElevatorBindings() { @@ -946,7 +944,7 @@ private void configureSoloControllerBindings() { switch (soloScoringMode) { case CORAL_IN_CLAW -> { scoreCommand = - getSoloCoralBranchHeightCommandL() + getCoralBranchHeightCommand("SL") .until( () -> soloController.a().getAsBoolean() @@ -1002,7 +1000,7 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommandR() + case CORAL_IN_CLAW -> getCoralBranchHeightCommand("SR") .until( () -> soloController.a().getAsBoolean() From 4663bbb78d7fe986348f10336efe0f1c8f5c972d Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 23:07:50 -0700 Subject: [PATCH 12/24] Removed auto score rubmle --- src/main/java/frc/robot/Controls.java | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 5c069acc..7691229e 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,15 +1,17 @@ package frc.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -38,7 +40,6 @@ import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; -import java.util.function.BooleanSupplier; public class Controls { private static final int SOLO_CONTROLLER_PORT = 0; @@ -1123,11 +1124,5 @@ private void configureSoloControllerBindings() { .startMovingVoltage( () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) .withName("Elevator Manual Control")); - // Ready to score rumble (Removal for autoscoring) - /* Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace()); - readyToScore.onTrue( - Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0))); - readyToScore.onFalse( - Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); */ } } From f6a454bbe7b0ae2b83fe22679bbfc5db39bed869 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 23:11:14 -0700 Subject: [PATCH 13/24] Correct use of ipScore repeat prescoreswing --- src/main/java/frc/robot/subsystems/SuperStructure.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 217675a6..3c2f084a 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -1,5 +1,9 @@ package frc.robot.subsystems; +import java.util.Set; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; @@ -9,9 +13,6 @@ import frc.robot.sensors.ElevatorLight; import frc.robot.sensors.IntakeSensor; import frc.robot.util.BranchHeight; -import java.util.Set; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; public class SuperStructure { private final ElevatorSubsystem elevator; @@ -72,7 +73,7 @@ private Command repeatPrescoreScoreSwing( // 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)); + command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(ipScore).until(score)); } else { return command.repeatedly().onlyWhile(armSensor.inClaw()); } From ba0ce5fe8af2d9c03c55c49b63077dada52a8c6b Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 23:14:38 -0700 Subject: [PATCH 14/24] Added comments for the in place scoring --- src/main/java/frc/robot/subsystems/SuperStructure.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 3c2f084a..09d94ab7 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -180,6 +180,10 @@ public Command coralLevelOne(BooleanSupplier score) { .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( From f3d86c85fde4454737aad3b1106274b0c316711c Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 23:52:41 -0700 Subject: [PATCH 15/24] Consolidated all autoAligns into 1 --- src/main/java/frc/robot/Controls.java | 47 +-- .../frc/robot/subsystems/SuperStructure.java | 14 +- .../frc/robot/subsystems/auto/AutoAlign.java | 298 ++++++------------ .../frc/robot/subsystems/auto/AutoLogic.java | 6 +- 4 files changed, 142 insertions(+), 223 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 7691229e..a75a7a48 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,17 +1,15 @@ package frc.robot; -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -40,6 +38,7 @@ import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; +import java.util.function.BooleanSupplier; public class Controls { private static final int SOLO_CONTROLLER_PORT = 0; @@ -459,13 +458,18 @@ private Command getCoralBranchHeightCommand(String version) { .coralLevelFour(soloController.rightBumper()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .coralLevelThree( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .coralLevelTwo( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L()) + .coralLevelOne( + soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } else if (version.equals("SR")) { @@ -474,13 +478,18 @@ private Command getCoralBranchHeightCommand(String version) { .coralLevelFour(soloController.rightBumper()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .coralLevelThree( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .coralLevelTwo( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R()) + .coralLevelOne( + soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } else { @@ -825,12 +834,12 @@ private void configureAutoAlignBindings() { .rightTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); driverController .leftTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); } private void configureGroundSpinnyBindings() { @@ -987,12 +996,12 @@ private void configureSoloControllerBindings() { .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); soloController .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignL1L(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -1029,12 +1038,12 @@ private void configureSoloControllerBindings() { .rightTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); soloController .rightTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignL1R(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB)); // Ground Intake soloController .leftBumper() diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 09d94ab7..41ec29d6 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -1,9 +1,5 @@ package frc.robot.subsystems; -import java.util.Set; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; @@ -13,6 +9,9 @@ import frc.robot.sensors.ElevatorLight; import frc.robot.sensors.IntakeSensor; import frc.robot.util.BranchHeight; +import java.util.Set; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; public class SuperStructure { private final ElevatorSubsystem elevator; @@ -73,7 +72,9 @@ private Command repeatPrescoreScoreSwing( // repeats scoring sequence if the coral is still in the claw if (armSensor == null) { return Commands.sequence( - command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(ipScore).until(score)); + command, + Commands.waitUntil(() -> !score.getAsBoolean()), + Commands.waitUntil(ipScore).until(score)); } else { return command.repeatedly().onlyWhile(armSensor.inClaw()); } @@ -181,7 +182,8 @@ public Command coralLevelOne(BooleanSupplier score) { } // 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 + // 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 diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 726558d8..344d3c92 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -19,28 +19,17 @@ import java.util.List; public class AutoAlign { - public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); + public enum AlignType { + ALLB, + LEFTB, + RIGHTB, + L1LB, + L1RB } - public static Command autoAlignL1L( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); - } - - public static Command autoAlignL1R( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); - } - - public static Command autoAlignLeft( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandLeft(drivebaseSubsystem, controls).withName("Auto Align"); - } - - public static Command autoAlignRight( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandRight(drivebaseSubsystem, controls).withName("Auto Align"); + public static Command autoAlign( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls, AlignType type) { + return new AutoAlignCommand(drivebaseSubsystem, controls, type).withName("Auto Align"); } public static Boolean isBlue() { @@ -55,19 +44,7 @@ public static Boolean isBlue() { } public static boolean readyToScore() { - return isStationary() && isLevel() && isCloseEnough(); - } - - public static boolean poseInPlace() { - return isStationary() && isCloseEnough(); - } - - public static boolean poseInPlaceL1L() { - return isStationary() && isCloseEnoughL1L(); - } - - public static boolean poseInPlaceL1R() { - return isStationary() && isCloseEnoughL1R(); + return isStationary() && isLevel() && isCloseEnough(AlignType.ALLB); } public static boolean isStationary() { @@ -83,27 +60,18 @@ public static boolean isLevel() { && MathUtil.isNear(0, rotation.getY(), Units.degreesToRadians(2)); } - public static boolean isCloseEnough() { + public static boolean isCloseEnough(AlignType type) { var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; - var branchPose = AutoAlignCommand.getNearestBranch(currentPose); + var branchPose = AutoAlignCommand.getTargetPose(currentPose, type); return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } - public static boolean isCloseEnoughL1L() { - var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; - var branchPose = AutoAlignCommandL1L.getNearestReefFace(currentPose); - return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; - } - - public static boolean isCloseEnoughL1R() { - var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; - var branchPose = AutoAlignCommandL1R.getNearestReefFace(currentPose); - return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + public static boolean poseInPlace(AlignType type) { + return isStationary() && isCloseEnough(type); } public static boolean oneSecondLeft() { // THIS WILL ONLY WORK ON THE REAL FIELD AND IN PRACTICE MODE! - return DriverStation.getMatchTime() <= 1; } @@ -225,41 +193,7 @@ public static boolean isCloseEnoughL1R() { private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); - private static final List blueBranchPoses = - List.of( - blueBranchA, - blueBranchB, - blueBranchC, - blueBranchD, - blueBranchE, - blueBranchF, - blueBranchG, - blueBranchH, - blueBranchI, - blueBranchJ, - blueBranchK, - blueBranchL); - ; - private static final List redBranchPoses = - List.of( - redBranchA, - redBranchB, - redBranchC, - redBranchD, - redBranchE, - redBranchF, - redBranchG, - redBranchH, - redBranchI, - redBranchJ, - redBranchK, - redBranchL); - private static class AutoAlignCommand extends Command { - public static Pose2d getNearestBranch(Pose2d p) { - List branchPose2ds = isBlue() ? blueBranchPoses : redBranchPoses; - return p.nearest(branchPose2ds); - } protected final PIDController pidX = new PIDController(4, 0, 0); protected final PIDController pidY = new PIDController(4, 0, 0); @@ -268,23 +202,25 @@ public static Pose2d getNearestBranch(Pose2d p) { protected final CommandSwerveDrivetrain drive; protected final Controls controls; protected Pose2d branchPose; + protected AlignType type; private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric() // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls) { + public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls, AlignType type) { this.drive = drive; pidRotate.enableContinuousInput(-Math.PI, Math.PI); this.controls = controls; + this.type = type; setName("Auto Align"); } @Override public void initialize() { Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestBranch(robotPose); + branchPose = getTargetPose(robotPose, type); pidX.setSetpoint(branchPose.getX()); pidY.setSetpoint(branchPose.getY()); pidRotate.setSetpoint(branchPose.getRotation().getRadians()); @@ -328,133 +264,105 @@ public void end(boolean interrupted) { drive.setControl(stop); controls.vibrateDriveController(0); } - } - private static class AutoAlignCommandLeft extends AutoAlignCommand { - private static final List blueLeftBranchPoses = - List.of(blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK); - - private static final List redLeftBranchPoses = - List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); - - public static Pose2d getNearestLeftBranch(Pose2d p, boolean isBlue) { - List branchPose2ds = isBlue ? blueLeftBranchPoses : redLeftBranchPoses; - return p.nearest(branchPose2ds); + 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 AutoAlignCommandLeft(CommandSwerveDrivetrain drive, Controls controls) { - super(drive, controls); - } - - @Override - public void initialize() { - Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestLeftBranch(robotPose, isBlue()); - pidX.setSetpoint(branchPose.getX()); - pidY.setSetpoint(branchPose.getY()); - pidRotate.setSetpoint(branchPose.getRotation().getRadians()); - } - } - - private static class AutoAlignCommandRight extends AutoAlignCommand { - private static final List blueRightBranchPoses = - List.of(blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL); - - private static final List redRightBranchPoses = - List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); - - public static Pose2d getNearestRightBranch(Pose2d p) { - List branchPose2ds = isBlue() ? blueRightBranchPoses : redRightBranchPoses; + private static Pose2d getNearestBranch(Pose2d p) { + List branchPose2ds = + 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); } - public AutoAlignCommandRight(CommandSwerveDrivetrain drive, Controls controls) { - super(drive, controls); + private static Pose2d getNearestLeftBranch(Pose2d p) { + List branchPose2ds = + isBlue() + ? List.of( + blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK) + : List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); + return p.nearest(branchPose2ds); } - @Override - public void initialize() { - Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestRightBranch(robotPose); - pidX.setSetpoint(branchPose.getX()); - pidY.setSetpoint(branchPose.getY()); - pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + private static Pose2d getNearestRightBranch(Pose2d p) { + List branchPose2ds = + isBlue() + ? List.of( + blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL) + : List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); + return p.nearest(branchPose2ds); } - } - private static class AutoAlignCommandL1L extends AutoAlignCommand { - private static final List blueReefFaces = - List.of( - lBlueReefFaceAB, - lBlueReefFaceCD, - lBlueReefFaceEF, - lBlueReefFaceGH, - lBlueReefFaceIJ, - lBlueReefFaceKL); - - private static final List redReefFaces = - List.of( - lRedReefFaceAB, - lRedReefFaceCD, - lRedReefFaceEF, - lRedReefFaceGH, - lRedReefFaceIJ, - lRedReefFaceKL); - - public static Pose2d getNearestReefFace(Pose2d p) { - List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + private static Pose2d getNearestL1L(Pose2d p) { + List reefFacesPose2ds = + isBlue() + ? List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL) + : List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); return p.nearest(reefFacesPose2ds); } - public AutoAlignCommandL1L(CommandSwerveDrivetrain drive, Controls controls) { - super(drive, controls); - } - - @Override - public void initialize() { - Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestReefFace(robotPose); - pidX.setSetpoint(branchPose.getX()); - pidY.setSetpoint(branchPose.getY()); - pidRotate.setSetpoint(branchPose.getRotation().getRadians()); - } - } - - private static class AutoAlignCommandL1R extends AutoAlignCommand { - private static final List blueReefFaces = - List.of( - rBlueReefFaceAB, - rBlueReefFaceCD, - rBlueReefFaceEF, - rBlueReefFaceGH, - rBlueReefFaceIJ, - rBlueReefFaceKL); - - private static final List redReefFaces = - List.of( - rRedReefFaceAB, - rRedReefFaceCD, - rRedReefFaceEF, - rRedReefFaceGH, - rRedReefFaceIJ, - rRedReefFaceKL); - - public static Pose2d getNearestReefFace(Pose2d p) { - List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + private static Pose2d getNearestL1R(Pose2d p) { + List reefFacesPose2ds = + isBlue() + ? List.of( + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL) + : List.of( + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); return p.nearest(reefFacesPose2ds); } - - public AutoAlignCommandL1R(CommandSwerveDrivetrain drive, Controls controls) { - super(drive, controls); - } - - @Override - public void initialize() { - Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestReefFace(robotPose); - pidX.setSetpoint(branchPose.getX()); - pidY.setSetpoint(branchPose.getY()); - pidRotate.setSetpoint(branchPose.getRotation().getRadians()); - } } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index 2f0a7a54..f65e48a9 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -229,7 +229,7 @@ public static void initShuffleBoard() { tab.add("Available Auto Variants", availableAutos).withPosition(4, 2).withSize(2, 1); tab.addBoolean("readyToScore?", () -> AutoAlign.readyToScore()); tab.addBoolean("Level?", () -> AutoAlign.isLevel()); - tab.addBoolean("Close Enough?", () -> AutoAlign.isCloseEnough()); + tab.addBoolean("Close Enough?", () -> AutoAlign.isCloseEnough(AutoAlign.AlignType.ALLB)); tab.addBoolean("Stationary?", () -> AutoAlign.isStationary()); tab.addBoolean("Low on time?", () -> AutoAlign.oneSecondLeft()); tab.addDouble("MATCH TIME(TIMER FOR AUTO)", () -> DriverStation.getMatchTime()); @@ -290,7 +290,7 @@ public static Command scoreCommand() { if (r.superStructure != null) { return new ConditionalCommand( // If true: - AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) .repeatedly() .withDeadline(r.superStructure.coralLevelFour(() -> AutoAlign.readyToScore())) .withName("scoreCommand"), @@ -299,7 +299,7 @@ public static Command scoreCommand() { // Condition: () -> ARMSENSOR_ENABLED && r.sensors.armSensor.booleanInClaw()); } - return AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + return AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) .withName("scoreCommand-noSuperstructure"); } From df050963b19507d669a41231ee9d849ac4f41efd Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Tue, 21 Oct 2025 00:14:28 -0700 Subject: [PATCH 16/24] Added static classes to check for alliance side. --- .../subsystems/auto/AutoAlgaeHeights.java | 13 +++-------- .../frc/robot/subsystems/auto/AutoAlign.java | 23 +++++-------------- .../subsystems/auto/AutoBuilderConfig.java | 9 ++------ .../java/frc/robot/util/AllianceUtils.java | 19 +++++++++++++++ 4 files changed, 30 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/util/AllianceUtils.java diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java index f3ee497a..4592ed7d 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -3,12 +3,11 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import frc.robot.util.ScoringMode; import java.util.List; @@ -54,20 +53,14 @@ public static Command autoAlgaeIntakeCommand( return new AutoAlgaeHeights(drivebase, s, c); } - private static boolean isBlue() { - return DriverStation.getAlliance() - .map(alliance -> alliance.equals(Alliance.Blue)) - .orElse(false); - } - private static Pose2d getNearestAlgae(Pose2d robotPose) { - List poses = isBlue() ? blueAlgaePoses : redAlgaePoses; + List poses = AllianceUtils.isBlue() ? blueAlgaePoses : redAlgaePoses; return robotPose.nearest(poses); } private String aprilTagToAlgaeHeight(Pose2d algaePose) { // Map poses to two heights only - List poses = isBlue() ? blueAlgaePoses : redAlgaePoses; + List poses = AllianceUtils.isBlue() ? blueAlgaePoses : redAlgaePoses; int index = poses.indexOf(algaePose); if (index >= 3) { diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 344d3c92..93df784a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -12,10 +12,10 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import java.util.List; public class AutoAlign { @@ -32,17 +32,6 @@ public static Command autoAlign( return new AutoAlignCommand(drivebaseSubsystem, controls, type).withName("Auto Align"); } - public static Boolean isBlue() { - boolean isBlue; - - if (!DriverStation.getAlliance().isEmpty()) { - isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); - } else { - isBlue = false; - } - return isBlue; - } - public static boolean readyToScore() { return isStationary() && isLevel() && isCloseEnough(AlignType.ALLB); } @@ -277,7 +266,7 @@ public static Pose2d getTargetPose(Pose2d pose, AlignType type) { private static Pose2d getNearestBranch(Pose2d p) { List branchPose2ds = - isBlue() + AllianceUtils.isBlue() ? List.of( blueBranchA, blueBranchB, @@ -309,7 +298,7 @@ private static Pose2d getNearestBranch(Pose2d p) { private static Pose2d getNearestLeftBranch(Pose2d p) { List branchPose2ds = - isBlue() + AllianceUtils.isBlue() ? List.of( blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK) : List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); @@ -318,7 +307,7 @@ private static Pose2d getNearestLeftBranch(Pose2d p) { private static Pose2d getNearestRightBranch(Pose2d p) { List branchPose2ds = - isBlue() + AllianceUtils.isBlue() ? List.of( blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL) : List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); @@ -327,7 +316,7 @@ private static Pose2d getNearestRightBranch(Pose2d p) { private static Pose2d getNearestL1L(Pose2d p) { List reefFacesPose2ds = - isBlue() + AllianceUtils.isBlue() ? List.of( lBlueReefFaceAB, lBlueReefFaceCD, @@ -347,7 +336,7 @@ private static Pose2d getNearestL1L(Pose2d p) { private static Pose2d getNearestL1R(Pose2d p) { List reefFacesPose2ds = - isBlue() + AllianceUtils.isBlue() ? List.of( rBlueReefFaceAB, rBlueReefFaceCD, diff --git a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java index 02f94323..e4eae0da 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java @@ -5,10 +5,10 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.Subsystems; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import java.io.IOException; import org.json.simple.parser.ParseException; @@ -42,12 +42,7 @@ public static void buildAuto(CommandSwerveDrivetrain drivebase) { // Boolean supplier that controls when the path will be mirrored for the red alliance // This will flip the path being followed to the red side of the field. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return !AutoAlign.isBlue(); // Checking alliance is red - } - return false; + return AllianceUtils.isRed(); // Checking alliance is red }, s.drivebaseSubsystem // Reference to this subsystem to set requirements ); diff --git a/src/main/java/frc/robot/util/AllianceUtils.java b/src/main/java/frc/robot/util/AllianceUtils.java new file mode 100644 index 00000000..1f0ef67e --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceUtils.java @@ -0,0 +1,19 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.DriverStation; + +public final class AllianceUtils { + public static boolean isBlue() { + if (!DriverStation.getAlliance().isEmpty()) { + return DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue); + } + return false; + } + + public static boolean isRed() { + if (!DriverStation.getAlliance().isEmpty()) { + return DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red); + } + return false; + } +} From eedbd42b67f4ff0ca9eacdbc3c861ff539fa3a5a Mon Sep 17 00:00:00 2001 From: Jetblackdragon Date: Fri, 24 Oct 2025 21:10:32 -0700 Subject: [PATCH 17/24] Removal of string checker. --- src/main/java/frc/robot/Controls.java | 15 ++++++++------- src/main/java/frc/robot/util/ScoringSide.java | 7 +++++++ 2 files changed, 15 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/util/ScoringSide.java diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index a75a7a48..2a7d04e9 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -39,6 +39,7 @@ import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; import java.util.function.BooleanSupplier; +import frc.robot.util.ScoringType; public class Controls { private static final int SOLO_CONTROLLER_PORT = 0; @@ -295,7 +296,7 @@ private void configureSuperStructureBindings() { Commands.deferredProxy( () -> switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand("2C"); + case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER); case ALGAE -> Commands.sequence( superStructure.algaeProcessorScore( driverController.rightBumper()), @@ -426,7 +427,7 @@ private void configureSuperStructureBindings() { () -> { Command scoreCommand = switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand("2C"); + case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER); case ALGAE -> Commands.sequence( BargeAlign.bargeScore( s.drivebaseSubsystem, @@ -451,8 +452,8 @@ private Command getAlgaeIntakeCommand() { }; } - private Command getCoralBranchHeightCommand(String version) { - if (version.equals("SL")) { + private Command getCoralBranchHeightCommand(ScoringType version) { + if (version == ScoringType.SOLOC_LEFT) { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) @@ -472,7 +473,7 @@ private Command getCoralBranchHeightCommand(String version) { soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB)) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; - } else if (version.equals("SR")) { + } else if (version == ScoringType.SOLOC_RIGHT) { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) @@ -954,7 +955,7 @@ private void configureSoloControllerBindings() { switch (soloScoringMode) { case CORAL_IN_CLAW -> { scoreCommand = - getCoralBranchHeightCommand("SL") + getCoralBranchHeightCommand(ScoringType.SOLOC_LEFT) .until( () -> soloController.a().getAsBoolean() @@ -1010,7 +1011,7 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getCoralBranchHeightCommand("SR") + case CORAL_IN_CLAW -> getCoralBranchHeightCommand(ScoringType.SOLOC_RIGHT) .until( () -> soloController.a().getAsBoolean() diff --git a/src/main/java/frc/robot/util/ScoringSide.java b/src/main/java/frc/robot/util/ScoringSide.java new file mode 100644 index 00000000..b99ffad9 --- /dev/null +++ b/src/main/java/frc/robot/util/ScoringSide.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum ScoringType { + SOLOC_RIGHT, + SOLOC_LEFT, + DRIVER; +} From 94c0abc9afba58f230064031106c2eb328111a8a Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Fri, 24 Oct 2025 21:14:14 -0700 Subject: [PATCH 18/24] spotless --- src/main/java/frc/robot/Controls.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 2a7d04e9..103385e2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -37,9 +37,9 @@ import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; +import frc.robot.util.ScoringType; import frc.robot.util.SoloScoringMode; import java.util.function.BooleanSupplier; -import frc.robot.util.ScoringType; public class Controls { private static final int SOLO_CONTROLLER_PORT = 0; @@ -1011,7 +1011,8 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getCoralBranchHeightCommand(ScoringType.SOLOC_RIGHT) + case CORAL_IN_CLAW -> getCoralBranchHeightCommand( + ScoringType.SOLOC_RIGHT) .until( () -> soloController.a().getAsBoolean() From fb705d2cb85f5fbf3c960ee91371a089d2c3c4bc Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Fri, 24 Oct 2025 21:19:47 -0700 Subject: [PATCH 19/24] name fix --- .../java/frc/robot/util/{ScoringSide.java => ScoringType.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/frc/robot/util/{ScoringSide.java => ScoringType.java} (100%) diff --git a/src/main/java/frc/robot/util/ScoringSide.java b/src/main/java/frc/robot/util/ScoringType.java similarity index 100% rename from src/main/java/frc/robot/util/ScoringSide.java rename to src/main/java/frc/robot/util/ScoringType.java From d5a696b29b26bac940fb5b1f87266a17bd3173eb Mon Sep 17 00:00:00 2001 From: iamawesomecat <145405364+iamawesomecat@users.noreply.github.com> Date: Thu, 11 Dec 2025 19:20:06 -0800 Subject: [PATCH 20/24] AutoAlign Cleanup --- .../frc/robot/subsystems/auto/AutoAlign.java | 168 ++++++++---------- 1 file changed, 76 insertions(+), 92 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 93df784a..fe73041a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -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); @@ -256,102 +325,17 @@ public void end(boolean interrupted) { 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); + 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); - } } } From 0813f592bc8974f7734d3578f9d12f0a3d4d4086 Mon Sep 17 00:00:00 2001 From: Jetblackdragon Date: Thu, 11 Dec 2025 19:20:06 -0800 Subject: [PATCH 21/24] AutoAlign Cleanup --- .../frc/robot/subsystems/auto/AutoAlign.java | 168 ++++++++---------- 1 file changed, 76 insertions(+), 92 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 93df784a..fe73041a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -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); @@ -256,102 +325,17 @@ public void end(boolean interrupted) { 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); + 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); - } } } From c3203230f83b4d68b19debe61b54d4897a2be080 Mon Sep 17 00:00:00 2001 From: iamawesomecat <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 18 Dec 2025 18:34:47 -0800 Subject: [PATCH 22/24] isConnected fix avoids calling isConnected 3 different times. --- src/main/java/frc/robot/Controls.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 0adda27d..ef4e6789 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 From 3f853fc2065909f6d5e81186ecdfbea4da408e55 Mon Sep 17 00:00:00 2001 From: iamawesomecat <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 18 Dec 2025 19:29:42 -0800 Subject: [PATCH 23/24] Coral Levels refactoring --- src/main/java/frc/robot/Controls.java | 9 +- .../frc/robot/subsystems/SuperStructure.java | 202 +++++------------- 2 files changed, 58 insertions(+), 153 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index ef4e6789..caacfe35 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -499,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) { From 0e3d33eae551307b965e8ad53bcd824fe049eaa4 Mon Sep 17 00:00:00 2001 From: iamawesomecat <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 18 Dec 2025 19:38:50 -0800 Subject: [PATCH 24/24] Change type to alignType for clarity --- .../frc/robot/subsystems/auto/AutoAlign.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index fe73041a..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 @@ -260,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()); @@ -323,8 +323,8 @@ public void end(boolean interrupted) { controls.vibrateDriveController(0); } - public static Pose2d getTargetPose(Pose2d pose, AlignType type) { - return switch (type) { + 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);