From 1f645e355ea127d181c2b60d81c64c6ffabc8c2e Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Tue, 9 Sep 2025 16:02:00 -0700 Subject: [PATCH 01/12] climb kP controller --- .../java/frc/robot/subsystems/ClimbPivot.java | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 5b10c18d..ff9fee92 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -83,6 +83,8 @@ public enum TargetPositions { // the target speed private double setSpeed = 0; + private double speedToSet = 0; + private double pError = 0; // alerts for checking if either motor is or isnt connected private final Alert NotConnectedErrorOne = @@ -390,12 +392,15 @@ public Command advanceClimbCheck() { setSpeed = 0; } else { if (!moveComplete) { - if (minTargetPos > getClimbPosition()) { - motorLeft.set(CLIMB_OUT_SPEED); - setSpeed = CLIMB_OUT_SPEED; + pError = minTargetPos-getClimbPosition(); + if (pError > 0) { + speedToSet = CLIMB_OUT_SPEED*pError; + motorLeft.set(speedToSet); + setSpeed = speedToSet; } else { - motorLeft.set(CLIMB_IN_SPEED); - setSpeed = CLIMB_IN_SPEED; + speedToSet = CLIMB_IN_SPEED*pError; + motorLeft.set(speedToSet); + setSpeed = speedToSet; } } } From 386fcf9d380ee478b04e41875815bb7af43a01ed Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Tue, 9 Sep 2025 16:09:06 -0700 Subject: [PATCH 02/12] remove algae mode coral ground intake --- src/main/java/frc/robot/Controls.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b7097488..0e4089b7 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -352,7 +352,16 @@ private void configureSuperStructureBindings() { driverController .b() .onTrue( - superStructure.quickGroundIntake(driverController.x()).withName("Quick Gound intake")); + Commands.runOnce( + () -> { + Command groundCommand = + switch(scoringMode) { + case CORAL -> superStructure.quickGroundIntake(driverController.x()).withName("Quick Gound intake"); + case ALGAE -> superStructure.supercycleGroundIntake(); + }; + CommandScheduler.getInstance().schedule(groundCommand); + })); + if (sensors.armSensor != null) { sensors From 6ec4f49fb65b6e3d24fd2dfb9b28c430c2d558d4 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Tue, 9 Sep 2025 16:24:14 -0700 Subject: [PATCH 03/12] hold coral while barge scoring --- .../frc/robot/subsystems/SuperStructure.java | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..ce8c03bf 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,6 +203,10 @@ public Command groundIntake(BooleanSupplier retract) { } } +public Command supercycleGroundIntake() { + return null; +} + // This is the actual version in use. It moves the coral directly into the claw. public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph if (groundSpinny == null || groundArm == null || intakeSensor == null) { @@ -370,7 +374,8 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR))) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR), + groundArm.moveToPosition(GroundArm.UP_POSITION))) .withName("Algae L3-L4 Intake"); } @@ -379,7 +384,8 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE))) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE), + groundArm.moveToPosition(GroundArm.STOWED_POSITION))) .withName("Algae L2-L3 Intake"); } @@ -388,7 +394,10 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.parallel( spinnyClaw.algaeIntakePower(), Commands.sequence( - armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), + Commands.parallel( + armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), + groundArm.moveToPosition(GroundArm.STOWED_POSITION) + ), elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } @@ -407,7 +416,10 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p Commands.parallel( spinnyClaw.algaeGripIntakePower(), Commands.sequence( - armPivot.moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE), + Commands.parallel( + armPivot.moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE), + groundArm.moveToPosition(GroundArm.STOWED_POSITION) + ), elevator.setLevel(ElevatorSubsystem.ALGAE_PROCESSOR_SCORE))), Commands.waitUntil(score), spinnyClaw.algaeExtakeProcessorPower()) @@ -417,9 +429,11 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p public Command algaeNetScore(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), + groundArm.moveToPosition(GroundArm.UP_POSITION), armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), - spinnyClaw.algaeIntakePower()), + spinnyClaw.algaeIntakePower() + ), + elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), Commands.waitUntil(score), spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), Commands.waitSeconds(0.7), From 990a636ae10f42f1c98db3d13d74c1b1f4794d23 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Wed, 10 Sep 2025 11:11:52 -0700 Subject: [PATCH 04/12] controls and style --- src/main/java/frc/robot/Controls.java | 17 +++++++++-------- .../frc/robot/subsystems/SuperStructure.java | 13 +++++-------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 0e4089b7..fc69d9d2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -353,15 +353,16 @@ private void configureSuperStructureBindings() { .b() .onTrue( Commands.runOnce( - () -> { - Command groundCommand = - switch(scoringMode) { - case CORAL -> superStructure.quickGroundIntake(driverController.x()).withName("Quick Gound intake"); + () -> { + Command groundCommand = + switch (scoringMode) { + case CORAL -> superStructure + .quickGroundIntake(driverController.x()) + .withName("Quick Gound intake"); case ALGAE -> superStructure.supercycleGroundIntake(); - }; - CommandScheduler.getInstance().schedule(groundCommand); - })); - + }; + CommandScheduler.getInstance().schedule(groundCommand); + })); if (sensors.armSensor != null) { sensors diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index ce8c03bf..05e93406 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,9 +203,9 @@ public Command groundIntake(BooleanSupplier retract) { } } -public Command supercycleGroundIntake() { + public Command supercycleGroundIntake() { return null; -} + } // This is the actual version in use. It moves the coral directly into the claw. public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph @@ -396,8 +396,7 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef Commands.sequence( Commands.parallel( armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), - groundArm.moveToPosition(GroundArm.STOWED_POSITION) - ), + groundArm.moveToPosition(GroundArm.STOWED_POSITION)), elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } @@ -418,8 +417,7 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p Commands.sequence( Commands.parallel( armPivot.moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE), - groundArm.moveToPosition(GroundArm.STOWED_POSITION) - ), + groundArm.moveToPosition(GroundArm.STOWED_POSITION)), elevator.setLevel(ElevatorSubsystem.ALGAE_PROCESSOR_SCORE))), Commands.waitUntil(score), spinnyClaw.algaeExtakeProcessorPower()) @@ -431,8 +429,7 @@ public Command algaeNetScore(BooleanSupplier score) { Commands.parallel( groundArm.moveToPosition(GroundArm.UP_POSITION), armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), - spinnyClaw.algaeIntakePower() - ), + spinnyClaw.algaeIntakePower()), elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), Commands.waitUntil(score), spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), From 0bbab2729805bcb7d2e3b9a2956a8cb03c11ca9b Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Wed, 10 Sep 2025 12:51:12 -0700 Subject: [PATCH 05/12] mostly working --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/Controls.java | 23 ++++++++++++++++--- .../java/frc/robot/subsystems/GroundArm.java | 5 ++-- .../frc/robot/subsystems/SuperStructure.java | 8 +++---- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cc7f2e5..a992d31a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "editor.tabSize": 2 + "editor.tabSize": 2, + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index fc69d9d2..3ff7c155 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -272,6 +272,8 @@ private void configureSuperStructureBindings() { .onTrue( selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); + + // Processor score and coral pre-score driverController .leftTrigger() .onTrue( @@ -288,6 +290,7 @@ private void configureSuperStructureBindings() { }) .withName("Schedule processor score")); + // Algae Mode operatorController .leftBumper() .onTrue( @@ -296,7 +299,10 @@ private void configureSuperStructureBindings() { .withName("Algae Scoring Mode")) .onTrue( Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand())) - .withName("run algae intake")); + .withName("run algae intake")) + .onTrue(Commands.runOnce(() -> s.groundArm.setDefaultCommand(s.groundArm.stop()))); + + // Coral Mode operatorController // should work??? .leftTrigger() .onTrue( @@ -304,7 +310,14 @@ private void configureSuperStructureBindings() { .alongWith(scoringModeSelectRumble()) .withName("Coral Scoring Mode")) .onTrue(superStructure.coralPreIntake()) - .onTrue(s.climbPivotSubsystem.toStow()); + .onTrue(s.climbPivotSubsystem.toStow()) + .onTrue( + s.groundArm + .moveToPosition(GroundArm.STOWED_POSITION) + .andThen(Commands.idle()) + .withName("Ground stowed position wait")); + + // Stow Mode operatorController .povLeft() .onTrue( @@ -315,6 +328,8 @@ private void configureSuperStructureBindings() { case ALGAE -> superStructure.algaeStow(); }) .withName("Stow")); + + // Stow algae, pre-intake coral operatorController .povDown() .onTrue( @@ -755,6 +770,7 @@ private void configureAutoAlignBindings() { .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + driverController .leftTrigger() .and(() -> scoringMode == ScoringMode.CORAL) @@ -766,7 +782,8 @@ private void configureGroundSpinnyBindings() { if (s.groundSpinny == null) { return; } - s.groundSpinny.setDefaultCommand(s.groundSpinny.holdFunnelIntakePower()); + s.groundSpinny.setDefaultCommand( + s.groundSpinny.holdFunnelIntakePower().unless(s.groundSpinny.stalling())); } private void configureGroundArmBindings() { diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 1e69da24..2f2fc403 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -27,7 +27,7 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KA = 0; public static final double STOWED_POSITION = 0.45; public static final double UP_POSITION = - 0.27; // untested - should be somewhere in between stowed and ground + 0.34; // untested - should be somewhere in between stowed and ground public static final double GROUND_POSITION = -0.050; public static final double QUICK_INTAKE_POSITION = 0.31; public static final double POS_TOLERANCE = Units.degreesToRotations(5); @@ -100,8 +100,8 @@ private double getTargetPosition() { private Command setTargetPosition(double pos) { return runOnce( () -> { - motor.setControl(m_request.withPosition(pos)); targetPos = pos; + motor.setControl(m_request.withPosition(pos)); }); } @@ -118,6 +118,7 @@ public void logTabs() { // preset command placeholder public Command moveToPosition(double position) { + targetPos = position; return setTargetPosition(position).andThen(Commands.waitUntil(atPosition(position))); } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 05e93406..4fc78cb2 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -374,8 +374,8 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR), - groundArm.moveToPosition(GroundArm.UP_POSITION))) + groundArm.moveToPosition(GroundArm.UP_POSITION)), + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) .withName("Algae L3-L4 Intake"); } @@ -384,8 +384,8 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE), - groundArm.moveToPosition(GroundArm.STOWED_POSITION))) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)), + groundArm.moveToPosition(GroundArm.STOWED_POSITION)) .withName("Algae L2-L3 Intake"); } From 6c0df5367f6867625e2b12eb10b5ede3c54bf3dd Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Wed, 10 Sep 2025 16:01:43 -0700 Subject: [PATCH 06/12] ground intake implemented --- src/main/java/frc/robot/Controls.java | 68 ++++++++----------- .../frc/robot/subsystems/SuperStructure.java | 26 +++++-- 2 files changed, 50 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 3ff7c155..51d20420 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -161,42 +161,6 @@ private void configureDrivebaseBindings() { .withRotationalRate(getDriveRotate())) .withName("Drive")); - // various former controls that were previously used and could be referenced in the future - - // operatorController - // .povUp() - // .whileTrue( - // s.drivebaseSubsystem - // .applyRequest( - // () -> - // drive - // .withVelocityX(MetersPerSecond.of(1.0)) - // .withVelocityY(0) - // .withRotationalRate(0)) - // .withName("1 m/s forward")); - // operatorController - // .povRight() - // .whileTrue( - // s.drivebaseSubsystem - // .applyRequest( - // () -> - // drive - // .withVelocityX(MetersPerSecond.of(2.0)) - // .withVelocityY(0) - // .withRotationalRate(0)) - // .withName("2 m/s forward")); - // driverController.a().whileTrue(s.drivebaseSubsystem.sysIdDynamic(Direction.kForward)); - // driverController.b().whileTrue(s.drivebaseSubsystem.sysIdDynamic(Direction.kReverse)); - // driverController.y().whileTrue(s.drivebaseSubsystem.sysIdQuasistatic(Direction.kForward)); - // driverController.x().whileTrue(s.drivebaseSubsystem.sysIdQuasistatic(Direction.kReverse)); - - // driveController.a().whileTrue(s.drivebaseSubsystem.applyRequest(() -> - // brake)); - // driveController.b().whileTrue(s.drivebaseSubsystem.applyRequest(() -> - // point.withModuleDirection(new Rotation2d(-driveController.getLeftY(), - // -driveController.getLeftX())) - // )); - // reset the field-centric heading on back button press driverController .back() @@ -225,48 +189,64 @@ private void configureSuperStructureBindings() { return; } superStructure.setBranchHeightSupplier(() -> branchHeight); + // operator start button used for climb - bound in climb bindings + // L4 operatorController .y() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) .withName("coral level 4, algae level 3-4")); + + // L3 operatorController .x() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 3, algae level 2-3")); + + // L2 operatorController .b() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 2, algae level 2-3")); + + // L1 operatorController .a() .onTrue( selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); + + // L4 driverController .povUp() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) .withName("coral level 4, algae level 3-4")); + + // L3 driverController .povLeft() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 3, algae level 2-3")); + + // L2 driverController .povRight() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 2, algae level 2-3")); + + // L1 driverController .povDown() .onTrue( @@ -315,7 +295,12 @@ private void configureSuperStructureBindings() { s.groundArm .moveToPosition(GroundArm.STOWED_POSITION) .andThen(Commands.idle()) - .withName("Ground stowed position wait")); + .withName("Ground stowed position wait")) + .onTrue( + Commands.runOnce( + () -> + s.groundArm.setDefaultCommand( + s.groundArm.moveToPosition(GroundArm.STOWED_POSITION)))); // Stow Mode operatorController @@ -341,6 +326,7 @@ private void configureSuperStructureBindings() { }) .withName("pre-intake, algae stow")); + // Driver manual coral intake driverController .a() .onTrue(s.elevatorSubsystem.runOnce(() -> {}).withName("elevator interruptor")) @@ -364,6 +350,7 @@ private void configureSuperStructureBindings() { }) .withName("Driver Intake")); + // Coral ground intake driverController .b() .onTrue( @@ -374,7 +361,9 @@ private void configureSuperStructureBindings() { case CORAL -> superStructure .quickGroundIntake(driverController.x()) .withName("Quick Gound intake"); - case ALGAE -> superStructure.supercycleGroundIntake(); + // Algae supercycling coral ground pickup, does not work with algae ground + // pickup lmao + case ALGAE -> superStructure.supercycleGroundIntake(driverController.x()); }; CommandScheduler.getInstance().schedule(groundCommand); })); @@ -782,8 +771,7 @@ private void configureGroundSpinnyBindings() { if (s.groundSpinny == null) { return; } - s.groundSpinny.setDefaultCommand( - s.groundSpinny.holdFunnelIntakePower().unless(s.groundSpinny.stalling())); + s.groundSpinny.setDefaultCommand(s.groundSpinny.holdFunnelIntakePower()); } private void configureGroundArmBindings() { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 4fc78cb2..25985279 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,8 +203,23 @@ public Command groundIntake(BooleanSupplier retract) { } } - public Command supercycleGroundIntake() { - return null; + public Command supercycleGroundIntake(BooleanSupplier retract) { + return // Core intake sequence + Commands.sequence( + // Deploy the ground arm (and wait until it reaches the position). + groundArm.moveToPosition(GroundArm.GROUND_POSITION), + // After it's deployed, apply a constant voltage to press it into the bumper + // and continue. + groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE), + // Once it's out, set the ground spinny speed + groundSpinny.setGroundIntakePower()) + + // Move on from the intake being down when stuff is triggered + .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))) + // And bring it back inside the robot + .andThen( + Commands.sequence( + groundArm.moveToPosition(GroundArm.UP_POSITION), groundSpinny.stop())); } // This is the actual version in use. It moves the coral directly into the claw. @@ -224,7 +239,9 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph 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(), // This used to stop the spinny, but now runs + // it so that coral stored in the ground intake can be re-picked up + // immediately 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)), @@ -239,7 +256,8 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph Commands.parallel( // These three are the initial setup: Move elevator down to the handoff // height, make sure armPivot finishes moving to the right height, and - // spin claw + // spin claw (which is left over from an older code, but is here as a + // backup) elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), spinnyClaw.coralIntakePower(), From 36173e519c19c0604dbf444586df419a381a7166 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Wed, 10 Sep 2025 16:15:46 -0700 Subject: [PATCH 07/12] Added and tuned climb p controller --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/subsystems/ClimbPivot.java | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cc7f2e5..a992d31a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "editor.tabSize": 2 + "editor.tabSize": 2, + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index ff9fee92..01fce3e2 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -56,6 +56,9 @@ public enum TargetPositions { private final double CLIMB_HOLD_CLIMBED = -0.0705; private final double CLIMB_IN_SPEED = -0.75; + private final double climbInKp = 30; + private final double climbOutKp = 50; + // positions for relation between motor encoder and WCP encoder // relative to eachother, likely not accurately zero'ed when obtained. private static final double MIN_ROTOR_POSITION = -50.45; @@ -392,13 +395,13 @@ public Command advanceClimbCheck() { setSpeed = 0; } else { if (!moveComplete) { - pError = minTargetPos-getClimbPosition(); + pError = minTargetPos - getClimbPosition(); if (pError > 0) { - speedToSet = CLIMB_OUT_SPEED*pError; + speedToSet = CLIMB_OUT_SPEED * pError * climbOutKp; motorLeft.set(speedToSet); setSpeed = speedToSet; } else { - speedToSet = CLIMB_IN_SPEED*pError; + speedToSet = CLIMB_IN_SPEED * -pError * climbInKp; motorLeft.set(speedToSet); setSpeed = speedToSet; } From 8af86a7c9cfdf745a76694a4469a10c268210e59 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Sun, 14 Sep 2025 19:18:38 -0700 Subject: [PATCH 08/12] Revert "Merge branch 'superCycling' into climbPivotAdvancements" This reverts commit b66aa89be3555d8751c75e51c8a273e906912bd9, reversing changes made to 36173e519c19c0604dbf444586df419a381a7166. --- src/main/java/frc/robot/Controls.java | 93 ++++++++----------- .../java/frc/robot/subsystems/GroundArm.java | 5 +- .../frc/robot/subsystems/SuperStructure.java | 43 ++------- 3 files changed, 48 insertions(+), 93 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 51d20420..b7097488 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -161,6 +161,42 @@ private void configureDrivebaseBindings() { .withRotationalRate(getDriveRotate())) .withName("Drive")); + // various former controls that were previously used and could be referenced in the future + + // operatorController + // .povUp() + // .whileTrue( + // s.drivebaseSubsystem + // .applyRequest( + // () -> + // drive + // .withVelocityX(MetersPerSecond.of(1.0)) + // .withVelocityY(0) + // .withRotationalRate(0)) + // .withName("1 m/s forward")); + // operatorController + // .povRight() + // .whileTrue( + // s.drivebaseSubsystem + // .applyRequest( + // () -> + // drive + // .withVelocityX(MetersPerSecond.of(2.0)) + // .withVelocityY(0) + // .withRotationalRate(0)) + // .withName("2 m/s forward")); + // driverController.a().whileTrue(s.drivebaseSubsystem.sysIdDynamic(Direction.kForward)); + // driverController.b().whileTrue(s.drivebaseSubsystem.sysIdDynamic(Direction.kReverse)); + // driverController.y().whileTrue(s.drivebaseSubsystem.sysIdQuasistatic(Direction.kForward)); + // driverController.x().whileTrue(s.drivebaseSubsystem.sysIdQuasistatic(Direction.kReverse)); + + // driveController.a().whileTrue(s.drivebaseSubsystem.applyRequest(() -> + // brake)); + // driveController.b().whileTrue(s.drivebaseSubsystem.applyRequest(() -> + // point.withModuleDirection(new Rotation2d(-driveController.getLeftY(), + // -driveController.getLeftX())) + // )); + // reset the field-centric heading on back button press driverController .back() @@ -189,71 +225,53 @@ private void configureSuperStructureBindings() { return; } superStructure.setBranchHeightSupplier(() -> branchHeight); - // operator start button used for climb - bound in climb bindings - // L4 operatorController .y() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) .withName("coral level 4, algae level 3-4")); - - // L3 operatorController .x() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 3, algae level 2-3")); - - // L2 operatorController .b() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 2, algae level 2-3")); - - // L1 operatorController .a() .onTrue( selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); - - // L4 driverController .povUp() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) .withName("coral level 4, algae level 3-4")); - - // L3 driverController .povLeft() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 3, algae level 2-3")); - - // L2 driverController .povRight() .onTrue( selectScoringHeight( BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) .withName("coral level 2, algae level 2-3")); - - // L1 driverController .povDown() .onTrue( selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); - - // Processor score and coral pre-score driverController .leftTrigger() .onTrue( @@ -270,7 +288,6 @@ private void configureSuperStructureBindings() { }) .withName("Schedule processor score")); - // Algae Mode operatorController .leftBumper() .onTrue( @@ -279,10 +296,7 @@ private void configureSuperStructureBindings() { .withName("Algae Scoring Mode")) .onTrue( Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand())) - .withName("run algae intake")) - .onTrue(Commands.runOnce(() -> s.groundArm.setDefaultCommand(s.groundArm.stop()))); - - // Coral Mode + .withName("run algae intake")); operatorController // should work??? .leftTrigger() .onTrue( @@ -290,19 +304,7 @@ private void configureSuperStructureBindings() { .alongWith(scoringModeSelectRumble()) .withName("Coral Scoring Mode")) .onTrue(superStructure.coralPreIntake()) - .onTrue(s.climbPivotSubsystem.toStow()) - .onTrue( - s.groundArm - .moveToPosition(GroundArm.STOWED_POSITION) - .andThen(Commands.idle()) - .withName("Ground stowed position wait")) - .onTrue( - Commands.runOnce( - () -> - s.groundArm.setDefaultCommand( - s.groundArm.moveToPosition(GroundArm.STOWED_POSITION)))); - - // Stow Mode + .onTrue(s.climbPivotSubsystem.toStow()); operatorController .povLeft() .onTrue( @@ -313,8 +315,6 @@ private void configureSuperStructureBindings() { case ALGAE -> superStructure.algaeStow(); }) .withName("Stow")); - - // Stow algae, pre-intake coral operatorController .povDown() .onTrue( @@ -326,7 +326,6 @@ private void configureSuperStructureBindings() { }) .withName("pre-intake, algae stow")); - // Driver manual coral intake driverController .a() .onTrue(s.elevatorSubsystem.runOnce(() -> {}).withName("elevator interruptor")) @@ -350,23 +349,10 @@ private void configureSuperStructureBindings() { }) .withName("Driver Intake")); - // Coral ground intake driverController .b() .onTrue( - Commands.runOnce( - () -> { - Command groundCommand = - switch (scoringMode) { - case CORAL -> superStructure - .quickGroundIntake(driverController.x()) - .withName("Quick Gound intake"); - // Algae supercycling coral ground pickup, does not work with algae ground - // pickup lmao - case ALGAE -> superStructure.supercycleGroundIntake(driverController.x()); - }; - CommandScheduler.getInstance().schedule(groundCommand); - })); + superStructure.quickGroundIntake(driverController.x()).withName("Quick Gound intake")); if (sensors.armSensor != null) { sensors @@ -759,7 +745,6 @@ private void configureAutoAlignBindings() { .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); - driverController .leftTrigger() .and(() -> scoringMode == ScoringMode.CORAL) diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 2f2fc403..1e69da24 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -27,7 +27,7 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KA = 0; public static final double STOWED_POSITION = 0.45; public static final double UP_POSITION = - 0.34; // untested - should be somewhere in between stowed and ground + 0.27; // untested - should be somewhere in between stowed and ground public static final double GROUND_POSITION = -0.050; public static final double QUICK_INTAKE_POSITION = 0.31; public static final double POS_TOLERANCE = Units.degreesToRotations(5); @@ -100,8 +100,8 @@ private double getTargetPosition() { private Command setTargetPosition(double pos) { return runOnce( () -> { - targetPos = pos; motor.setControl(m_request.withPosition(pos)); + targetPos = pos; }); } @@ -118,7 +118,6 @@ public void logTabs() { // preset command placeholder public Command moveToPosition(double position) { - targetPos = position; return setTargetPosition(position).andThen(Commands.waitUntil(atPosition(position))); } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 25985279..d37b680b 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,25 +203,6 @@ public Command groundIntake(BooleanSupplier retract) { } } - public Command supercycleGroundIntake(BooleanSupplier retract) { - return // Core intake sequence - Commands.sequence( - // Deploy the ground arm (and wait until it reaches the position). - groundArm.moveToPosition(GroundArm.GROUND_POSITION), - // After it's deployed, apply a constant voltage to press it into the bumper - // and continue. - groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE), - // Once it's out, set the ground spinny speed - groundSpinny.setGroundIntakePower()) - - // Move on from the intake being down when stuff is triggered - .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))) - // And bring it back inside the robot - .andThen( - Commands.sequence( - groundArm.moveToPosition(GroundArm.UP_POSITION), groundSpinny.stop())); - } - // This is the actual version in use. It moves the coral directly into the claw. public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph if (groundSpinny == null || groundArm == null || intakeSensor == null) { @@ -239,9 +220,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(), // This used to stop the spinny, but now runs - // it so that coral stored in the ground intake can be re-picked up - // immediately + spinnyClaw.stop(), // just as a backup in case things are silly 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)), @@ -256,8 +235,7 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph Commands.parallel( // These three are the initial setup: Move elevator down to the handoff // height, make sure armPivot finishes moving to the right height, and - // spin claw (which is left over from an older code, but is here as a - // backup) + // spin claw elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), spinnyClaw.coralIntakePower(), @@ -392,8 +370,7 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - groundArm.moveToPosition(GroundArm.UP_POSITION)), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR))) .withName("Algae L3-L4 Intake"); } @@ -402,8 +379,7 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)), - groundArm.moveToPosition(GroundArm.STOWED_POSITION)) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE))) .withName("Algae L2-L3 Intake"); } @@ -412,9 +388,7 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.parallel( spinnyClaw.algaeIntakePower(), Commands.sequence( - Commands.parallel( - armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), - groundArm.moveToPosition(GroundArm.STOWED_POSITION)), + armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } @@ -433,9 +407,7 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p Commands.parallel( spinnyClaw.algaeGripIntakePower(), Commands.sequence( - Commands.parallel( - armPivot.moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE), - groundArm.moveToPosition(GroundArm.STOWED_POSITION)), + armPivot.moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE), elevator.setLevel(ElevatorSubsystem.ALGAE_PROCESSOR_SCORE))), Commands.waitUntil(score), spinnyClaw.algaeExtakeProcessorPower()) @@ -445,10 +417,9 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p public Command algaeNetScore(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - groundArm.moveToPosition(GroundArm.UP_POSITION), + elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), spinnyClaw.algaeIntakePower()), - elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), Commands.waitUntil(score), spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), Commands.waitSeconds(0.7), From a307498016543a6bb1e614a89fdea8ed142fdeaf Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Sun, 14 Sep 2025 19:28:57 -0700 Subject: [PATCH 09/12] undo default command switching --- src/main/java/frc/robot/Controls.java | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 51d20420..c69ace4d 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -279,8 +279,7 @@ private void configureSuperStructureBindings() { .withName("Algae Scoring Mode")) .onTrue( Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand())) - .withName("run algae intake")) - .onTrue(Commands.runOnce(() -> s.groundArm.setDefaultCommand(s.groundArm.stop()))); + .withName("run algae intake")); // Coral Mode operatorController // should work??? @@ -295,12 +294,7 @@ private void configureSuperStructureBindings() { s.groundArm .moveToPosition(GroundArm.STOWED_POSITION) .andThen(Commands.idle()) - .withName("Ground stowed position wait")) - .onTrue( - Commands.runOnce( - () -> - s.groundArm.setDefaultCommand( - s.groundArm.moveToPosition(GroundArm.STOWED_POSITION)))); + .withName("Ground stowed position wait")); // Stow Mode operatorController From eedf1439132bef98a46b24c53cb61a1992123ab6 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Sun, 14 Sep 2025 21:35:35 -0700 Subject: [PATCH 10/12] ground intake hold position now --- src/main/java/frc/robot/Controls.java | 3 +-- .../java/frc/robot/subsystems/GroundArm.java | 1 - .../frc/robot/subsystems/GroundSpinny.java | 5 +++++ .../frc/robot/subsystems/SuperStructure.java | 19 +++++++++++-------- 4 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index c69ace4d..2b7e33ba 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -293,8 +293,7 @@ private void configureSuperStructureBindings() { .onTrue( s.groundArm .moveToPosition(GroundArm.STOWED_POSITION) - .andThen(Commands.idle()) - .withName("Ground stowed position wait")); + .withName("Ground stowed position")); // Stow Mode operatorController diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 2f2fc403..d9c53b73 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -118,7 +118,6 @@ public void logTabs() { // preset command placeholder public Command moveToPosition(double position) { - targetPos = position; return setTargetPosition(position).andThen(Commands.waitUntil(atPosition(position))); } diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 93ce77e4..6f67f220 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -17,6 +17,7 @@ public class GroundSpinny extends SubsystemBase { public static final double FUNNEL_INTAKE_SPEED = -2; public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1; private static final double STATOR_CURRENT_STALL_THRESHOLD = 20; + public static final double GROUND_INTAKE_HOLD_SPEED = -1; // TalonFX private final TalonFX motor; @@ -80,6 +81,10 @@ public Command holdFunnelIntakePower() { return holdPower(FUNNEL_INTAKE_SPEED).withName("hold funnel intake power"); } + public Command holdCoralPower() { + return holdPower(GROUND_INTAKE_HOLD_SPEED).withName("hold coral holding power"); + } + public void imperativeSetGroundIntakePower() { motor.setVoltage(GROUND_INTAKE_SPEED); lastSetPower = GROUND_INTAKE_SPEED; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 25985279..0ba2cce3 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -208,18 +208,21 @@ public Command supercycleGroundIntake(BooleanSupplier retract) { Commands.sequence( // Deploy the ground arm (and wait until it reaches the position). groundArm.moveToPosition(GroundArm.GROUND_POSITION), + // Once it's out, set the ground spinny speed + groundSpinny.setGroundIntakePower(), // After it's deployed, apply a constant voltage to press it into the bumper // and continue. - groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE), - // Once it's out, set the ground spinny speed - groundSpinny.setGroundIntakePower()) + groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE)) // Move on from the intake being down when stuff is triggered .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))) // And bring it back inside the robot .andThen( Commands.sequence( - groundArm.moveToPosition(GroundArm.UP_POSITION), groundSpinny.stop())); + // Move ground intake to in positiong and hold it there + groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle()), + // Set ground intake power to just hold the coral tightly + groundSpinny.holdCoralPower())); } // This is the actual version in use. It moves the coral directly into the claw. @@ -392,7 +395,7 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - groundArm.moveToPosition(GroundArm.UP_POSITION)), + groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle())), elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) .withName("Algae L3-L4 Intake"); } @@ -403,7 +406,7 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)), - groundArm.moveToPosition(GroundArm.STOWED_POSITION)) + groundArm.moveToPosition(GroundArm.STOWED_POSITION).andThen(Commands.idle())) .withName("Algae L2-L3 Intake"); } @@ -414,7 +417,7 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef Commands.sequence( Commands.parallel( armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), - groundArm.moveToPosition(GroundArm.STOWED_POSITION)), + groundArm.moveToPosition(GroundArm.STOWED_POSITION).andThen(Commands.idle())), elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } @@ -445,7 +448,7 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p public Command algaeNetScore(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - groundArm.moveToPosition(GroundArm.UP_POSITION), + groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle()), armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), spinnyClaw.algaeIntakePower()), elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), From 5559a0d86a112966cecc84fa6f868397f3646570 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Mon, 15 Sep 2025 14:51:45 -0700 Subject: [PATCH 11/12] supercycling auto handoff --- src/main/java/frc/robot/Controls.java | 31 +++++++++++++++-- .../java/frc/robot/sensors/IntakeSensor.java | 5 +++ .../java/frc/robot/subsystems/GroundArm.java | 4 +-- .../frc/robot/subsystems/SuperStructure.java | 33 +++++++++++++++++++ 4 files changed, 69 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 4a1b56e0..70ae6466 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.generated.BonkTunerConstants; import frc.robot.generated.CompTunerConstants; +import frc.robot.sensors.IntakeSensor; import frc.robot.subsystems.ArmPivot; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.GroundArm; @@ -272,6 +273,8 @@ private void configureSuperStructureBindings() { .onTrue( selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); + + //Processor scoring and left coral pre-score driverController .leftTrigger() .onTrue( @@ -283,11 +286,22 @@ private void configureSuperStructureBindings() { superStructure.algaeProcessorScore( driverController.rightBumper()), Commands.waitSeconds(0.7), - getAlgaeIntakeCommand()) + Commands.deferredProxy( + () -> switch (sensors.intakeSensor.inIntakeSwitchable()) { + case 1 -> + Commands.sequence( + Commands.runOnce(() -> scoringMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure.superCycleCoralHandoff() + .withName("supercycle handoff")); + default -> getAlgaeIntakeCommand(); + })) .withName("Processor score"); }) .withName("Schedule processor score")); + //Algae mode operatorController .leftBumper() .onTrue( @@ -379,6 +393,8 @@ private void configureSuperStructureBindings() { : Commands.none()) .withName("Automatic Intake")); } + + //Net scoring and right coral pre-score driverController .rightTrigger() .onTrue( @@ -395,7 +411,18 @@ private void configureSuperStructureBindings() { () -> getDriveY(), () -> getDriveRotate(), driverController.rightBumper()), - getAlgaeIntakeCommand()) + //If we are supercycling (have a coral), hand it off and switch to coral mode + Commands.deferredProxy( + () -> switch (sensors.intakeSensor.inIntakeSwitchable()) { + case 1 -> + Commands.sequence( + Commands.runOnce(() -> scoringMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure.superCycleCoralHandoff() + .withName("supercycle handoff")); + default -> getAlgaeIntakeCommand(); + })) .withName("Algae score then intake"); }; CommandScheduler.getInstance().schedule(scoreCommand); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 314c6035..35d3796f 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -59,4 +59,9 @@ public Trigger inIntake() { }) .debounce(0.1); } + + public int inIntakeSwitchable() { + double distance = getSensorDistance().in(Meters); + return (distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT) ? 1 : 0; + } } diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 1e69da24..789ab558 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -26,8 +26,8 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KG = 0.048; private final double ARMPIVOT_KA = 0; public static final double STOWED_POSITION = 0.45; - public static final double UP_POSITION = - 0.27; // untested - should be somewhere in between stowed and ground + public static final double UP_POSITION = 0.25; // Tested and working for supercycling + public static final double MIN_OUT_QUICK_HANDOFF = 0.15; //Needs to be tested, used by supercycling public static final double GROUND_POSITION = -0.050; public static final double QUICK_INTAKE_POSITION = 0.31; public static final double POS_TOLERANCE = Units.degreesToRotations(5); diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 2b05e9f0..1a783dcf 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -225,6 +225,39 @@ public Command supercycleGroundIntake(BooleanSupplier retract) { groundSpinny.holdCoralPower())); } + //This is called when a coral is in the ground intake and the robot has just scored an algae + public Command superCycleCoralHandoff() { + 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 + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.stop(), // just as a backup in case things are silly + groundSpinny.setGroundIntakePower()) + // Move on even if arm isn't in position yet as long as elevator is high enough + .until(elevator.above(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE)), + //Move the ground intake out + groundArm.moveToPosition(GroundArm.MIN_OUT_QUICK_HANDOFF), + //Prep is done, now do the handoff + Commands.parallel( + // These three are the initial setup: Move elevator down to the handoff + // height, make sure armPivot finishes moving to the right height, and + // spin claw + elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.coralIntakePower()), + //Start the sequence of handing off the coral + groundArm + .moveToPosition(GroundArm.STOWED_POSITION) + .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)), + // Spin groundSpinny out, but skip if we lost the coral. + groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), + // Go back to stow, but skip if we lost the coral. + coralStow().onlyIf(armSensor.inClaw()) + ); + } + // This is the actual version in use. It moves the coral directly into the claw. public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph if (groundSpinny == null || groundArm == null || intakeSensor == null) { From dc7735120b0596a1019efe914d3c5ed740163d61 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Mon, 15 Sep 2025 14:52:51 -0700 Subject: [PATCH 12/12] style --- src/main/java/frc/robot/Controls.java | 46 ++++++++++--------- .../java/frc/robot/sensors/IntakeSensor.java | 2 +- .../java/frc/robot/subsystems/GroundArm.java | 3 +- .../frc/robot/subsystems/SuperStructure.java | 43 +++++++++-------- 4 files changed, 49 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 70ae6466..9eb88b68 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -26,7 +26,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.generated.BonkTunerConstants; import frc.robot.generated.CompTunerConstants; -import frc.robot.sensors.IntakeSensor; import frc.robot.subsystems.ArmPivot; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.GroundArm; @@ -274,7 +273,7 @@ private void configureSuperStructureBindings() { selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); - //Processor scoring and left coral pre-score + // Processor scoring and left coral pre-score driverController .leftTrigger() .onTrue( @@ -287,21 +286,23 @@ private void configureSuperStructureBindings() { driverController.rightBumper()), Commands.waitSeconds(0.7), Commands.deferredProxy( - () -> switch (sensors.intakeSensor.inIntakeSwitchable()) { - case 1 -> - Commands.sequence( - Commands.runOnce(() -> scoringMode = ScoringMode.CORAL) + () -> + switch (sensors.intakeSensor.inIntakeSwitchable()) { + case 1 -> Commands.sequence( + Commands.runOnce( + () -> scoringMode = ScoringMode.CORAL) .alongWith(scoringModeSelectRumble()) .withName("Coral Scoring Mode"), - superStructure.superCycleCoralHandoff() + superStructure + .superCycleCoralHandoff() .withName("supercycle handoff")); default -> getAlgaeIntakeCommand(); - })) + })) .withName("Processor score"); }) .withName("Schedule processor score")); - //Algae mode + // Algae mode operatorController .leftBumper() .onTrue( @@ -394,7 +395,7 @@ private void configureSuperStructureBindings() { .withName("Automatic Intake")); } - //Net scoring and right coral pre-score + // Net scoring and right coral pre-score driverController .rightTrigger() .onTrue( @@ -411,17 +412,20 @@ private void configureSuperStructureBindings() { () -> getDriveY(), () -> getDriveRotate(), driverController.rightBumper()), - //If we are supercycling (have a coral), hand it off and switch to coral mode - Commands.deferredProxy( - () -> switch (sensors.intakeSensor.inIntakeSwitchable()) { - case 1 -> - Commands.sequence( - Commands.runOnce(() -> scoringMode = ScoringMode.CORAL) - .alongWith(scoringModeSelectRumble()) - .withName("Coral Scoring Mode"), - superStructure.superCycleCoralHandoff() - .withName("supercycle handoff")); - default -> getAlgaeIntakeCommand(); + // If we are supercycling (have a coral), hand it off and switch + // to coral mode + Commands.deferredProxy( + () -> + switch (sensors.intakeSensor.inIntakeSwitchable()) { + case 1 -> Commands.sequence( + Commands.runOnce( + () -> scoringMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure + .superCycleCoralHandoff() + .withName("supercycle handoff")); + default -> getAlgaeIntakeCommand(); })) .withName("Algae score then intake"); }; diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 35d3796f..b21a604c 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -62,6 +62,6 @@ public Trigger inIntake() { public int inIntakeSwitchable() { double distance = getSensorDistance().in(Meters); - return (distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT) ? 1 : 0; + return (distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT) ? 1 : 0; } } diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 789ab558..a1eafbb1 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -27,7 +27,8 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KA = 0; public static final double STOWED_POSITION = 0.45; public static final double UP_POSITION = 0.25; // Tested and working for supercycling - public static final double MIN_OUT_QUICK_HANDOFF = 0.15; //Needs to be tested, used by supercycling + public static final double MIN_OUT_QUICK_HANDOFF = + 0.15; // Needs to be tested, used by supercycling public static final double GROUND_POSITION = -0.050; public static final double QUICK_INTAKE_POSITION = 0.31; public static final double POS_TOLERANCE = Units.degreesToRotations(5); diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 1a783dcf..c7d71428 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -225,37 +225,36 @@ public Command supercycleGroundIntake(BooleanSupplier retract) { groundSpinny.holdCoralPower())); } - //This is called when a coral is in the ground intake and the robot has just scored an algae + // This is called when a coral is in the ground intake and the robot has just scored an algae public Command superCycleCoralHandoff() { 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 - Commands.parallel( + // arm pivot, stop the spinny claw, and start spinning the ground intake + Commands.parallel( elevator.setLevel(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), spinnyClaw.stop(), // just as a backup in case things are silly groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough .until(elevator.above(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE)), - //Move the ground intake out - groundArm.moveToPosition(GroundArm.MIN_OUT_QUICK_HANDOFF), - //Prep is done, now do the handoff - Commands.parallel( - // These three are the initial setup: Move elevator down to the handoff - // height, make sure armPivot finishes moving to the right height, and - // spin claw - elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), - armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), - spinnyClaw.coralIntakePower()), - //Start the sequence of handing off the coral - groundArm - .moveToPosition(GroundArm.STOWED_POSITION) - .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)), - // Spin groundSpinny out, but skip if we lost the coral. - groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), - // Go back to stow, but skip if we lost the coral. - coralStow().onlyIf(armSensor.inClaw()) - ); + // Move the ground intake out + groundArm.moveToPosition(GroundArm.MIN_OUT_QUICK_HANDOFF), + // Prep is done, now do the handoff + Commands.parallel( + // These three are the initial setup: Move elevator down to the handoff + // height, make sure armPivot finishes moving to the right height, and + // spin claw + elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.coralIntakePower()), + // Start the sequence of handing off the coral + groundArm + .moveToPosition(GroundArm.STOWED_POSITION) + .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)), + // Spin groundSpinny out, but skip if we lost the coral. + groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), + // Go back to stow, but skip if we lost the coral. + coralStow().onlyIf(armSensor.inClaw())); } // This is the actual version in use. It moves the coral directly into the claw.