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/27] 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/27] 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/27] 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/27] 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/27] 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/27] 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/27] 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/27] 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/27] 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/27] 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 820357e84041cc5b7320516e0f92b7325baf4e9e Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 15:13:16 -0700 Subject: [PATCH 11/27] current limits for motors --- .../java/frc/robot/generated/CompTunerConstants.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 179d2b32..4f0e2f4a 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -80,7 +80,12 @@ public class CompTunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration().withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(160)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(100)) + .withSupplyCurrentLimitEnable(true));; private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -89,7 +94,9 @@ public class CompTunerConstants { // low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(40)) + .withSupplyCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; From 9f588051cc09177b5fd7419ee9b45aa631bc273b Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 15:17:53 -0700 Subject: [PATCH 12/27] Velocity control --- src/main/java/frc/robot/generated/CompTunerConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 4f0e2f4a..f6220067 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -54,7 +54,7 @@ public class CompTunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + new Slot0Configs().withKP(0.2).withKI(0).withKD(0).withKS(0.1).withKV(0.124).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors From 7bc25978a205d8b7f803b8f8b3839585904e55c1 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 15:18:08 -0700 Subject: [PATCH 13/27] VC --- src/main/java/frc/robot/Controls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 36eb85d2..b99e2d53 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -84,7 +84,7 @@ public class Controls { .withDeadband(0.0001) .withRotationalDeadband(0.0001) .withDriveRequestType( - DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + DriveRequestType.Velocity); private final Telemetry logger = new Telemetry(MaxSpeed); From 7d02c1d5d9724a358f1fa99f80ebcf5bc2ed853f Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 20 Oct 2025 17:34:53 -0700 Subject: [PATCH 14/27] aarta sketchy drivebase change TEST AT NEPF 10/21 --- src/main/java/frc/robot/Controls.java | 3 +-- .../frc/robot/generated/CompTunerConstants.java | 16 +++++++++------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b99e2d53..3a0c187b 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -83,8 +83,7 @@ public class Controls { new SwerveRequest.FieldCentric() .withDeadband(0.0001) .withRotationalDeadband(0.0001) - .withDriveRequestType( - DriveRequestType.Velocity); + .withDriveRequestType(DriveRequestType.Velocity); private final Telemetry logger = new Telemetry(MaxSpeed); diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index f6220067..882f9998 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -76,16 +76,18 @@ public class CompTunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(80.0); + private static final Current kSlipCurrent = Amps.of(70.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration().withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(Amps.of(160)) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(Amps.of(100)) - .withSupplyCurrentLimitEnable(true));; + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(70)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(55)) + .withSupplyCurrentLimitEnable(true)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( From 3c38d31f27391ef8c04f81407c56838470b26348 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 20 Oct 2025 22:55:04 -0700 Subject: [PATCH 15/27] rebump up of drive motor current limits --- src/main/java/frc/robot/generated/CompTunerConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 882f9998..a9394ce9 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -76,7 +76,7 @@ public class CompTunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(70.0); + private static final Current kSlipCurrent = Amps.of(80.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -84,9 +84,9 @@ public class CompTunerConstants { new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(Amps.of(70)) + .withStatorCurrentLimit(Amps.of(160)) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(Amps.of(55)) + .withSupplyCurrentLimit(Amps.of(100)) .withSupplyCurrentLimitEnable(true)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() 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 16/27] 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 17/27] 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 18/27] 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 19/27] 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 20/27] 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 05605aeccf2f06fb77a8dbd21432f935abdff1fc 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 21/27] 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 f4a5bae9a93016f5919a69be6104c605c8a8ffd0 Mon Sep 17 00:00:00 2001 From: DriverStation3 <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 23 Oct 2025 20:27:54 -0700 Subject: [PATCH 22/27] shop testing changes 10/23 --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/subsystems/ArmPivot.java | 4 ++-- src/main/java/frc/robot/subsystems/SpinnyClaw.java | 5 +++++ .../java/frc/robot/subsystems/auto/BargeAlign.java | 14 +++++++------- 4 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..388ea6a4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -52,6 +53,7 @@ protected Robot() { PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); + RobotController.setBrownoutVoltage(6.3); sensors = new Sensors(); subsystems = new Subsystems(sensors); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index bdb98cae..6d98d800 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -195,9 +195,9 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 20; // previously 40 + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 30; // previously 40 talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 10; // previously 20 + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; // previously 20 talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e8c15b5f..b225ca2a 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -30,6 +30,7 @@ public class SpinnyClaw extends SubsystemBase { public static final double ALGAE_EXTAKE_SPEED = 14; public static final double ALGAE_PROCESSOR_EXTAKE_SPEED = 8; public static final double ALGAE_FLING_SPEED = 10; + public static final double CORAL_GRIP_INTAKE_SPEED = -0.5; // Remove once we implement PID speed public static int placeholderPIDSpeed; @@ -185,6 +186,10 @@ public Command stop() { return runOnce(() -> motor.stopMotor()).withName("Spinny claw stop"); } + public Command coralGripIntakePower() { + return setPower(CORAL_GRIP_INTAKE_SPEED).withName("Coral grip intake power"); + } + @Override public void periodic() { NotConnectedError.set(notConnectedDebouncer.calculate(!motor.getMotorVoltage().hasUpdated())); diff --git a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java index 0e87d818..8930731b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java @@ -48,7 +48,7 @@ private static Command driveToBlackLine( .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - private static final double xBargeDriveSpeed = 0.5; + private static final double xBargeDriveSpeed = 1.5; public static Command bargeScore( CommandSwerveDrivetrain drivebaseSubsystem, @@ -58,16 +58,16 @@ public static Command bargeScore( DoubleSupplier manualRotateSpeed, BooleanSupplier manualScore) { return Commands.sequence( - BargeAlign.driveToBlackLine( - drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) - .asProxy(), + // BargeAlign.driveToBlackLine( + // drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) + // .asProxy(), BargeAlign.driveToBarge(drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) .asProxy() .withDeadline( superStructure.algaeNetScore( - () -> - manualScore.getAsBoolean() - || BargeAlign.atScoringXPosition(drivebaseSubsystem)))); + () -> manualScore.getAsBoolean() + // || BargeAlign.atScoringXPosition(drivebaseSubsystem) + ))); } private static Command driveToBarge( From f7b07a7cf2d1d92d8037d724ac28eeafaa3d24d4 Mon Sep 17 00:00:00 2001 From: DriverStation3 <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 23 Oct 2025 20:29:46 -0700 Subject: [PATCH 23/27] SPTOELSSSSS --- src/main/java/frc/robot/subsystems/SuperStructure.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 41ec29d6..b24c2697 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -63,7 +63,10 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) return Commands.sequence( command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score)); } else { - return command.repeatedly().onlyWhile(armSensor.inClaw()); + return Commands.parallel( + Commands.sequence( + spinnyClaw.coralGripIntakePower(), Commands.waitSeconds(0.15), spinnyClaw.stop()), + command.repeatedly().onlyWhile(armSensor.inClaw())); } } From acb8bde23b941d83d37348b54421b509778112fd Mon Sep 17 00:00:00 2001 From: DriverStation3 <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 23 Oct 2025 20:30:31 -0700 Subject: [PATCH 24/27] Timing fix --- src/main/java/frc/robot/subsystems/SuperStructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index b24c2697..444ee47d 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -65,7 +65,7 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) } else { return Commands.parallel( Commands.sequence( - spinnyClaw.coralGripIntakePower(), Commands.waitSeconds(0.15), spinnyClaw.stop()), + spinnyClaw.coralGripIntakePower(), Commands.waitSeconds(0.05), spinnyClaw.stop()), command.repeatedly().onlyWhile(armSensor.inClaw())); } } From 8b80718124a08dbd427a5e8d3e43be0d2efb2c1d Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 25 Oct 2025 08:24:14 -0700 Subject: [PATCH 25/27] elastic layout override --- src/main/deploy/elastic-layout.json | 54 +++++------------------------ 1 file changed, 8 insertions(+), 46 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 40397540..7da24551 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -422,32 +422,6 @@ "data_type": "double", "show_submit_button": false } - }, - { - "title": "photonvision_Port_1182_Output_MJPEG_Server", - "x": 256.0, - "y": 512.0, - "width": 384.0, - "height": 384.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", - "period": 0.06, - "rotation_turns": 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 - } } ] } @@ -602,29 +576,17 @@ } }, { - "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, + "title": "streams", + "x": 128.0, "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", + "width": 128.0, + "height": 128.0, + "type": "Text Display", "properties": { - "topic": "/CameraPublisher/right", + "topic": "/CameraPublisher/Arducam_OV9281R/streams", "period": 0.06, - "rotation_turns": 0 + "data_type": "string[]", + "show_submit_button": false } } ] From 71242583e5213c777f579793130c015a6a1231e7 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 25 Oct 2025 08:55:26 -0700 Subject: [PATCH 26/27] pre comp changes --- src/main/deploy/elastic-layout.json | 1416 +++++++++++++---- .../frc/robot/subsystems/auto/AutoAlign.java | 2 +- .../frc/robot/subsystems/auto/BargeAlign.java | 4 +- 3 files changed, 1091 insertions(+), 331 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 40397540..304a189b 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -3,33 +3,33 @@ "grid_size": 128, "tabs": [ { - "name": "Teleoperated", - "grid_layout": { - "layouts": [], - "containers": [] - } - }, - { - "name": "Autonomous", - "grid_layout": { - "layouts": [], - "containers": [] - } - }, - { - "name": "ArmSensor", + "name": "Elevator", "grid_layout": { "layouts": [], "containers": [ { - "title": "inTrough", - "x": 0.0, + "title": "Elevator Speed", + "x": 640.0, "y": 0.0, - "width": 128.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/ArmSensor/inTrough", + "topic": "/Shuffleboard/Elevator/Is Zeroed", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -37,24 +37,16 @@ "true_icon": "None", "false_icon": "None" } - } - ] - } - }, - { - "name": "IntakeSensor", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "In Intake", + "title": "M1 at forward softstop", "x": 0.0, - "y": 0.0, - "width": 128.0, + "y": 128.0, + "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/IntakeSensor/In Intake", + "topic": "/Shuffleboard/Elevator/M1 at forward softstop", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -62,181 +54,212 @@ "true_icon": "None", "false_icon": "None" } - } - ] - } - }, - { - "name": "BranchSensor", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "LeftDistance(m)", + "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/BranchSensor/LeftDistance(m)", + "topic": "/Shuffleboard/Elevator/M1 output voltage", "period": 0.06, "data_type": "double", "show_submit_button": false } - } - ] - } - }, - { - "name": "AprilTags", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "Last raw timestamp", - "x": 0.0, + "title": "M1 supply current", + "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/AprilTags/Last raw timestamp", + "topic": "/Shuffleboard/Elevator/M1 supply current", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Num targets", - "x": 0.0, + "title": "M2 at forward softstop", + "x": 1280.0, "y": 128.0, - "width": 128.0, + "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/AprilTags/Num targets", + "topic": "/Shuffleboard/Elevator/M2 at forward softstop", "period": 0.06, - "data_type": "int", - "show_submit_button": false + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Last timestamp", - "x": 128.0, + "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/AprilTags/Last timestamp", + "topic": "/Shuffleboard/Elevator/M2 output voltage", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "april tag distance meters", - "x": 128.0, - "y": 128.0, + "title": "M2 supply current", + "x": 1280.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/AprilTags/april tag distance meters", + "topic": "/Shuffleboard/Elevator/M2 supply current", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "time since last reading", - "x": 256.0, + "title": "M2 temp", + "x": 1152.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/AprilTags/time since last reading", + "topic": "/Shuffleboard/Elevator/M2 temp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Disable vision", - "x": 512.0, + "title": "M1 temp", + "x": 256.0, "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Toggle Button", + "width": 128.0, + "height": 128.0, + "type": "Text Display", "properties": { - "topic": "/Shuffleboard/AprilTags/Disable vision", + "topic": "/Shuffleboard/Elevator/M1 temp", "period": 0.06, - "data_type": "boolean" + "data_type": "double", + "show_submit_button": false } } ] } }, { - "name": "Elevator", + "name": "Autos", "grid_layout": { "layouts": [], "containers": [ { - "title": "Motor Current Position", - "x": 0.0, - "y": 0.0, - "width": 128.0, + "title": "Auto Delay", + "x": 640.0, + "y": 384.0, + "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Elevator/Motor Current Position", + "topic": "/Shuffleboard/Autos/Auto Delay", "period": 0.06, "data_type": "double", "show_submit_button": false } - } - ] - } - }, - { - "name": "Arm Pivot", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "Pivot Speed", - "x": 0.0, + "title": "Starting Position", + "x": 512.0, "y": 0.0, - "width": 128.0, + "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "topic": "/Shuffleboard/Autos/Starting Position", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } - } - ] - } - }, - { - "name": "Climb", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "Is Climb OUT?", - "x": 0.0, + "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/Climb/Is Climb OUT?", + "topic": "/Shuffleboard/Autos/Close Enough?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -244,107 +267,46 @@ "true_icon": "None", "false_icon": "None" } - } - ] - } - }, - { - "name": "Claw", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "Claw Speed", - "x": 0.0, + "title": "Level?", + "x": 1280.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Claw/Claw Speed", + "topic": "/Shuffleboard/Autos/Level?", "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" } - } - ] - } - }, - { - "name": "GroundIntake", - "grid_layout": { - "layouts": [], - "containers": [ + }, { - "title": "Speed", - "x": 0.0, + "title": "Stationary?", + "x": 1408.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/GroundIntake/Speed", + "topic": "/Shuffleboard/Autos/Stationary?", "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - } - ] - } - }, - { - "name": "Ground Arm", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Pivot Speed", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - } - ] - } - }, - { - "name": "Controls", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Swerve Coast Mode", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Toggle Button", - "properties": { - "topic": "/Shuffleboard/Controls/Swerve Coast Mode", - "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } - } - ] - } - }, - { - "name": "Autos", - "grid_layout": { - "layouts": [], - "containers": [ + }, { "title": "readyToScore?", - "x": 0.0, - "y": 0.0, - "width": 128.0, + "x": 1152.0, + "y": 128.0, + "width": 384.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -358,95 +320,77 @@ } }, { - "title": "Starting Position", - "x": 512.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", + "title": "Selected auto", + "x": 0.0, + "y": 256.0, + "width": 512.0, + "height": 384.0, + "type": "Field", "properties": { - "topic": "/Shuffleboard/Autos/Starting Position", + "topic": "/Shuffleboard/Field/Selected auto", "period": 0.06, - "sort_options": false + "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": "Launch Type", - "x": 512.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "ComboBox Chooser", + "title": "Start pose", + "x": 1024.0, + "y": 256.0, + "width": 512.0, + "height": 384.0, + "type": "Field", "properties": { - "topic": "/Shuffleboard/Autos/Launch Type", + "topic": "/Shuffleboard/Field/Start pose", "period": 0.06, - "sort_options": false + "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": "Game Objects", - "x": 640.0, + "title": "Auto display speed", + "x": 0.0, "y": 128.0, - "width": 128.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": 256.0, "width": 256.0, "height": 128.0, - "type": "ComboBox Chooser", + "type": "Number Slider", "properties": { - "topic": "/Shuffleboard/Autos/Available Auto Variants", + "topic": "/Shuffleboard/Field/Auto display speed", "period": 0.06, - "sort_options": false + "data_type": "double", + "min_value": -0.5, + "max_value": 3.0, + "divisions": 5, + "update_continuously": false } }, { - "title": "Auto Delay", - "x": 512.0, - "y": 384.0, + "title": "Low on time?", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Autos/Auto Delay", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "photonvision_Port_1182_Output_MJPEG_Server", - "x": 256.0, - "y": 512.0, - "width": 384.0, - "height": 384.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "photonvision_Port_1186_Output_MJPEG_Server", - "x": 640.0, - "y": 512.0, - "width": 384.0, - "height": 384.0, - "type": "Camera Stream", + "type": "Boolean Box", "properties": { - "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", + "topic": "/Shuffleboard/Autos/Low on time?", "period": 0.06, - "rotation_turns": 0 + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } } ] @@ -512,119 +456,935 @@ } }, { - "name": "Match", + "name": "AprilTags", "grid_layout": { "layouts": [], "containers": [ { - "title": "has arm sensor", + "title": "Last raw timestamp", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Boolean Box", + "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Match/has arm sensor", + "topic": "/Shuffleboard/AprilTags/Last raw timestamp", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "data_type": "double", + "show_submit_button": false } }, { - "title": "has ground intake sensor", - "x": 128.0, - "y": 0.0, + "title": "Num targets", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Boolean Box", + "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Match/has ground intake sensor", + "topic": "/Shuffleboard/AprilTags/Num targets", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "data_type": "int", + "show_submit_button": false } }, { - "title": "has left branch sensor", - "x": 0.0, - "y": 128.0, + "title": "Last timestamp", + "x": 128.0, + "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Boolean Box", + "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Match/has left branch sensor", + "topic": "/Shuffleboard/AprilTags/Last timestamp", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "data_type": "double", + "show_submit_button": false } }, { - "title": "has right branch sensor", + "title": "april tag distance meters", "x": 128.0, "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Boolean Box", + "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Match/has right branch sensor", + "topic": "/Shuffleboard/AprilTags/april tag distance meters", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "data_type": "double", + "show_submit_button": false } }, { - "title": "has recent vision measurements", - "x": 384.0, + "title": "time since last reading", + "x": 256.0, "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", + "width": 128.0, + "height": 128.0, + "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Match/has recent vision measurements", + "topic": "/Shuffleboard/AprilTags/time since last reading", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "data_type": "double", + "show_submit_button": false } }, { - "title": "left cam", - "x": 256.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", + "title": "Disable vision", + "x": 512.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Toggle Button", "properties": { - "topic": "/CameraPublisher/left", + "topic": "/Shuffleboard/AprilTags/Disable vision", "period": 0.06, - "rotation_turns": 0 + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Controls", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Elevator Coast Mode", + "x": 384.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Elevator Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Swerve Coast Mode", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Swerve Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climb Coast Mode", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Climb Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Teleop time", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Controls/Teleop time", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Match", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "has arm sensor", + "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": "Is Zeroed", + "x": 640.0, + "y": 0.0, + "width": 256.0, + "height": 256.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" + } + } + ] + } + }, + { + "name": "Climb", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Is Climb OUT?", + "x": 640.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Climb/Is Climb OUT?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Is Climb STOWED?", + "x": 640.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", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Speed", + "x": 1152.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Climb/Motor 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" + } + }, + { + "title": "Set speed", + "x": 1280.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", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Where moving?", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Climb/Where moving?", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "GroundIntake", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor Temperature", + "x": 0.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Motor Temperature", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor rotor Pos", + "x": 128.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Motor rotor Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Position", + "x": 256.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Speed", + "x": 384.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Target Pos", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Target Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Last Set Power", + "x": 0.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Last Set Power", + "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", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Motor Temperature", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Distance(m)", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/IntakeSensor/Distance(m)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "In Intake", + "x": 768.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/IntakeSensor/In Intake", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "End Effector", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pivot Target Pos", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Target Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Speed", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Position", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor rotor Pos", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Motor rotor Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor Temperature", + "x": 384.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)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, { - "title": "right cam", + "title": "inClaw", + "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": 512.0, - "height": 512.0, - "type": "Camera Stream", + "width": 256.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", + "properties": { + "topic": "/Shuffleboard/Claw/Claw Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Last Set Power", + "x": 1152.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Claw/Last Set Power", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 1408.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Claw/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Tab 10", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Tab 11", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "ArmSensor", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "inTrough", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.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" + } + } + ] + } + }, + { + "name": "IntakeSensor", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "In Intake", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/IntakeSensor/In Intake", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "BranchSensor", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "LeftDistance(m)", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/BranchSensor/LeftDistance(m)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Arm Pivot", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Claw", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Claw Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Claw/Claw Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Ground Arm", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", "properties": { - "topic": "/CameraPublisher/right", + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", "period": 0.06, - "rotation_turns": 0 + "data_type": "double", + "show_submit_button": false } } ] diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 93df784a..7d28f1c5 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -195,7 +195,7 @@ private static class AutoAlignCommand extends Command { private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric() // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDriveRequestType(DriveRequestType.Velocity) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls, AlignType type) { diff --git a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java index 8930731b..401a4f5c 100644 --- a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java @@ -24,7 +24,7 @@ public class BargeAlign extends Command { private final SwerveRequest.FieldCentric blackLineDriveRequest = new SwerveRequest.FieldCentric() - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDriveRequestType(DriveRequestType.Velocity) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); public static boolean atScoringXPosition(CommandSwerveDrivetrain drivebasesubsystem) { @@ -45,7 +45,7 @@ private static Command driveToBlackLine( new SwerveRequest.FieldCentric() .withDeadband(0.0001) .withRotationalDeadband(0.0001) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDriveRequestType(DriveRequestType.Velocity) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); private static final double xBargeDriveSpeed = 1.5; From 0e252e2a3d77a506fa1e61b9a298fccc49af7e93 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 6 Nov 2025 19:19:23 -0800 Subject: [PATCH 27/27] double jump auto --- .../pathplanner/autos/Double-Bump-JK.auto | 81 +++++++++++++++++++ .../deploy/pathplanner/paths/DoubleBump.path | 54 +++++++++++++ .../frc/robot/subsystems/SuperStructure.java | 5 +- .../frc/robot/subsystems/auto/AutoLogic.java | 3 +- 4 files changed, 141 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Double-Bump-JK.auto create mode 100644 src/main/deploy/pathplanner/paths/DoubleBump.path diff --git a/src/main/deploy/pathplanner/autos/Double-Bump-JK.auto b/src/main/deploy/pathplanner/autos/Double-Bump-JK.auto new file mode 100644 index 00000000..3e9bbbe6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Double-Bump-JK.auto @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "DoubleBump" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LSF to K" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DoubleBump.path b/src/main/deploy/pathplanner/paths/DoubleBump.path new file mode 100644 index 00000000..94ca3be6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DoubleBump.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.187, + "y": 6.171 + }, + "prevControl": null, + "nextControl": { + "x": 10.110439640764461, + "y": 6.844424581380097 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.436266447368421, + "y": 6.1802631578947365 + }, + "prevControl": { + "x": 4.436266447368421, + "y": 6.1802631578947365 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -123.69006752597963 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 444ee47d..c06fa2ad 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -79,7 +79,10 @@ private Command repeatPrescoreScoreSwing( Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(ipScore).until(score)); } else { - return command.repeatedly().onlyWhile(armSensor.inClaw()); + return Commands.parallel( + Commands.sequence( + spinnyClaw.coralGripIntakePower(), Commands.waitSeconds(0.05), spinnyClaw.stop()), + command.repeatedly().onlyWhile(armSensor.inClaw())); } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index f65e48a9..4d738018 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -111,7 +111,8 @@ public static enum StartPosition { new AutoPath("MRSF_G-F_WithWait", "MRSF_G-F_WithWait"), new AutoPath("MRSF_G-H", "MRSF_G-H"), new AutoPath("MLSF_H-K_Cooking", "MLSF_H-K_Cooking"), - new AutoPath("MLSF_H-G", "MLSF_H-G")); + new AutoPath("MLSF_H-G", "MLSF_H-G"), + new AutoPath("Double-Bump-JK", "Double-Bump-JK")); private static List threePiecePaths = List.of(