From 4dd715431b22e2acb202c42b5b86ecf4b0faaa16 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sat, 28 Mar 2026 09:18:53 -0500 Subject: [PATCH 01/11] Test commit --- src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java index 0926be7..7daf779 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java @@ -15,6 +15,7 @@ import frc.robot.Configs; import frc.robot.Constants; +// Test public class IntakePivotSubsystem extends SubsystemBase { public TalonFX intake; private final PositionVoltage angleRequest = new PositionVoltage(0); From cf991c614decbddc9dc306429a480acbce8df254 Mon Sep 17 00:00:00 2001 From: N/A Date: Sat, 28 Mar 2026 12:45:11 -0500 Subject: [PATCH 02/11] fix: add 2s delay to auto to let shooter rev up and prevent jams --- src/main/java/frc/robot/commands/ShootPreloadCommand.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java index 8bde4fd..ed51c7b 100644 --- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java +++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java @@ -5,10 +5,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.IntakePivotSubsystem; import frc.robot.subsystems.IntakeRollerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -32,9 +31,10 @@ public ShootPreloadCommand( addCommands( Commands.deadline( - Commands.waitSeconds(shootPreloadTime), + Commands.waitSeconds(shootPreloadTime + 2), turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), + new WaitCommand(2), // transfer.getLoadCommand(), intakePivot.deployIntakeCommand(), intakeRoller.getIntakeCommand() From 82b1f60ab16c7c66458919dee51f0bc57cc52af9 Mon Sep 17 00:00:00 2001 From: N/A Date: Sat, 28 Mar 2026 14:44:46 -0500 Subject: [PATCH 03/11] feat(auto): deploy intake before shooting delay --- src/main/java/frc/robot/commands/ShootPreloadCommand.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java index ed51c7b..98237a0 100644 --- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java +++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java @@ -34,10 +34,10 @@ public ShootPreloadCommand( Commands.waitSeconds(shootPreloadTime + 2), turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), - new WaitCommand(2), // - transfer.getLoadCommand(), + intakeRoller.getIntakeCommand(), intakePivot.deployIntakeCommand(), - intakeRoller.getIntakeCommand() + new WaitCommand(2), // + transfer.getLoadCommand() ), Commands.parallel( From 4900911e0fc0efe552bf562318fa11f9960ad1c1 Mon Sep 17 00:00:00 2001 From: N/A Date: Sat, 28 Mar 2026 14:46:32 -0500 Subject: [PATCH 04/11] fix: flip depot and outpost autos when on red --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e402363..c7ca529 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -587,8 +587,8 @@ private Command getIntakeStrategyCommand() { turretSubsystem, transferSubsystem, () -> FieldUtil.flipIfRed(Constants.Field.BLUE_HUB_TRANSLATION), - Constants.Operator.Auto.DEPOT_READY_INTAKE_POSE, - Constants.Operator.Auto.DEPOT_INTAKE_POSE, + FieldUtil.flipIfRed(Constants.Operator.Auto.DEPOT_READY_INTAKE_POSE), + FieldUtil.flipIfRed(Constants.Operator.Auto.DEPOT_INTAKE_POSE), false, Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED, SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME)); @@ -602,8 +602,8 @@ private Command getIntakeStrategyCommand() { turretSubsystem, transferSubsystem, () -> FieldUtil.flipIfRed(Constants.Field.BLUE_HUB_TRANSLATION), - Constants.Operator.Auto.OUTPOST_READY_INTAKE_POSE, - Constants.Operator.Auto.OUTPOST_INTAKE_POSE, + FieldUtil.flipIfRed(Constants.Operator.Auto.OUTPOST_READY_INTAKE_POSE), + FieldUtil.flipIfRed(Constants.Operator.Auto.OUTPOST_INTAKE_POSE), false, Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED, SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME)); From e7be1897006b8f8dfcf2253af5cfffbfb48b6797 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sat, 28 Mar 2026 14:59:29 -0500 Subject: [PATCH 05/11] Increase power offset rate. Add buttons to reset offsets. --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++++++++++++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c51cc7..99ca347 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -58,7 +58,7 @@ public static class ErrorSettings { public static final int TURRET_OFFSET_DECIMAL_PLACE = 1; public static final double HOOD_OFFSET_INCREASE = 0.1; public static final int HOOD_OFFSET_DECIMAL_PLACE = 1; - public static final double SHOOTER_PERCENT_INCREASE = 0.2; // Percent to increase per tick + public static final double SHOOTER_PERCENT_INCREASE = 2; // Percent to increase per tick public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1; public static final double SHOOTER_PERCENT_DEFAULT = 100.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e402363..29ca267 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -388,6 +388,30 @@ public void configureBindings() { shooterPercent.getHeldIntervalCommand(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_INCREASE, Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); + // ============================== + // Turret / Hood Offset Reset (Left Stick Click / Right Stick Click) + // ============================== + + // Left Stick Click : Reset turret offset to zero + new Trigger(() -> operatorController.getLeftStickButton()) + .onTrue(new InstantCommand(() -> turretOffsetDegrees.set(0.0))); + + // Right Stick Click : Reset hood offset to zero + new Trigger(() -> operatorController.getRightStickButton()) + .onTrue(new InstantCommand(() -> hoodOffsetDegrees.set(0.0))); + + // ============================== + // Shooter Percent Reset (Left Trigger / Right Trigger) + // ============================== + + // Left Trigger : Reset shooter percent to default + new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5) + .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT))); + + // Right Trigger : Reset shooter percent to default + new Trigger(() -> operatorController.getRightTriggerAxis() > 0.5) + .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT))); + new Trigger( () -> operatorController.getAButton()).onTrue( new InstantCommand(() -> selectedFixedTarget = FixedTarget.A)); From 249cb68a4b68e6455b728860f6a637a1dc748c98 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sat, 28 Mar 2026 15:03:06 -0500 Subject: [PATCH 06/11] Remove test comment --- src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java index 7daf779..0926be7 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java @@ -15,7 +15,6 @@ import frc.robot.Configs; import frc.robot.Constants; -// Test public class IntakePivotSubsystem extends SubsystemBase { public TalonFX intake; private final PositionVoltage angleRequest = new PositionVoltage(0); From cc4c03480ec92fe70769b2a9cefe1609deae8344 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sat, 28 Mar 2026 15:15:42 -0500 Subject: [PATCH 07/11] Drop to 1 percent per tick for shooter percentage --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99ca347..1a3b8ca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -58,7 +58,7 @@ public static class ErrorSettings { public static final int TURRET_OFFSET_DECIMAL_PLACE = 1; public static final double HOOD_OFFSET_INCREASE = 0.1; public static final int HOOD_OFFSET_DECIMAL_PLACE = 1; - public static final double SHOOTER_PERCENT_INCREASE = 2; // Percent to increase per tick + public static final double SHOOTER_PERCENT_INCREASE = 1; // Percent to increase per tick public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1; public static final double SHOOTER_PERCENT_DEFAULT = 100.0; From fc752159cd8c30666bbcbf1b22600312359ac43f Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sat, 28 Mar 2026 15:29:05 -0500 Subject: [PATCH 08/11] Combine reset for both triggers into one command Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29ca267..e58238e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -404,14 +404,10 @@ public void configureBindings() { // Shooter Percent Reset (Left Trigger / Right Trigger) // ============================== - // Left Trigger : Reset shooter percent to default - new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5) + // Either Trigger : Reset shooter percent to default + new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5 + || operatorController.getRightTriggerAxis() > 0.5) .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT))); - - // Right Trigger : Reset shooter percent to default - new Trigger(() -> operatorController.getRightTriggerAxis() > 0.5) - .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT))); - new Trigger( () -> operatorController.getAButton()).onTrue( new InstantCommand(() -> selectedFixedTarget = FixedTarget.A)); From c7fa2eaadffed2cc3b41aec92c66bad07869f508 Mon Sep 17 00:00:00 2001 From: N/A Date: Sat, 28 Mar 2026 15:29:45 -0500 Subject: [PATCH 09/11] fix: correct 2 second delay issue --- .../frc/robot/commands/ShootPreloadCommand.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java index 98237a0..2207b2c 100644 --- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java +++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.IntakePivotSubsystem; @@ -32,12 +33,16 @@ public ShootPreloadCommand( Commands.deadline( Commands.waitSeconds(shootPreloadTime + 2), - turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), - shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), - intakeRoller.getIntakeCommand(), - intakePivot.deployIntakeCommand(), - new WaitCommand(2), // - transfer.getLoadCommand() + new SequentialCommandGroup( + new ParallelCommandGroup( + turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), + shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier), + intakeRoller.getIntakeCommand(), + intakePivot.deployIntakeCommand() + ), + new WaitCommand(2), + transfer.getLoadCommand() + ) ), Commands.parallel( From 6fba5825a7727c0fa6b9a16a4026602fed27c820 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sat, 28 Mar 2026 15:54:21 -0500 Subject: [PATCH 10/11] 0.5 percent per 20 ms for shooter power --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1a3b8ca..5507c65 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -58,7 +58,7 @@ public static class ErrorSettings { public static final int TURRET_OFFSET_DECIMAL_PLACE = 1; public static final double HOOD_OFFSET_INCREASE = 0.1; public static final int HOOD_OFFSET_DECIMAL_PLACE = 1; - public static final double SHOOTER_PERCENT_INCREASE = 1; // Percent to increase per tick + public static final double SHOOTER_PERCENT_INCREASE = .5; // Percent to increase per tick public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1; public static final double SHOOTER_PERCENT_DEFAULT = 100.0; From 2e94cb2603096859ff4ed102e5632cc0d0032472 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Sun, 29 Mar 2026 08:33:25 -0500 Subject: [PATCH 11/11] Move x lock to driver Y button --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7324569..a35061a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -283,8 +283,8 @@ public void configureBindings() { Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton()); - // Driver X button: hold to lock robot pose (X-lock) - Trigger xLockTrigger = new Trigger(() -> operatorController.getXButton()); + // Driver Y button: hold to lock robot pose (X-lock) + Trigger xLockTrigger = new Trigger(() -> driverController.getYButton()); xLockTrigger.whileTrue(driveSubsystem.getLockPoseCommand()); /* Shooter runs while button held */