From ad0a34572bc57133a8c8c8c9c12468817f121150 Mon Sep 17 00:00:00 2001 From: David Polcen Date: Tue, 7 Apr 2026 18:40:12 -0500 Subject: [PATCH 1/7] feat: updated the turret default command to getStow and added delay a 0.1 second delay to the transfer activating --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 11 ++++++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 87eacde..6c9df24 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -460,6 +460,8 @@ public static class Transfer { public static final boolean MOTOR_INVERTED = true; public static final double LOAD_POWER = 0.25; + + public static final double LOAD_DELAY = 0.1; //Time between shooter and transfer being triggered, shooter should always activate first } public static class VisionOdometryConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3d93968..236c7d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -287,7 +288,10 @@ public void configureBindings() { /* Transfer runs ONLY while button AND solver valid */ shootTrigger .whileTrue( - transferSubsystem.getLoadCommand()); + new WaitCommand(Constants.Transfer.LOAD_DELAY).andThen( + transferSubsystem.getLoadCommand() + )); + /* Intake pivot runs while button held */ intakeTrigger.whileTrue( @@ -515,10 +519,7 @@ public void scheduleTeleOp() { manualRotationVelocityDirective, null, null); driveSubsystem.setDefaultCommand(manualDriveCommand); - turretSubsystem.setDefaultCommand(turretSubsystem.getTargetCommand( - targetingSupplier, - driveSubsystem::getPose, - driveSubsystem::getVelocity)); + turretSubsystem.setDefaultCommand(turretSubsystem.getStowCommand()); } From fc4c00764d53a9e8d61edd645c7fcaaf9a9143a5 Mon Sep 17 00:00:00 2001 From: David Polcen Date: Tue, 7 Apr 2026 20:20:06 -0500 Subject: [PATCH 2/7] Improved the logic for the getStow command --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 8 +++++ .../frc/robot/subsystems/TurretSubsystem.java | 34 +++++++++---------- 3 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c9df24..d6e8d43 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -390,6 +390,8 @@ public static class Turret { public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(5.00); public static final Rotation2d HOOD_STOW_POSITION = Rotation2d.fromDegrees(3.00); + public static final double HOOD_FINISH_VELOCITY = 0.0; + public static final double OUTPUT_RANGE_MAX = 1; public static final double OUTPUT_RANGE_MIN = -1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 236c7d7..a68a1a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -285,6 +285,14 @@ public void configureBindings() { driveSubsystem::getPose, driveSubsystem::getVelocity)); + shootTrigger.whileTrue( + turretSubsystem.getTargetCommand( + targetingSupplier, + driveSubsystem::getPose, + driveSubsystem::getVelocity + ) + ); + /* Transfer runs ONLY while button AND solver valid */ shootTrigger .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index d79fbb3..d84d60d 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -168,6 +168,10 @@ public Rotation2d getHoodAngle() { - hoodOffsetSupplier.get().getRadians()); } + public double getHoodVelocity() { + return pitchEncoder.getVelocity().getValueAsDouble() * Constants.Turret.PITCH_ENCODER_FACTOR; + } + /** * @return the current turret yaw in the field frame */ @@ -269,32 +273,26 @@ public Command getAngleCommand( public Command getStowCommand() { - Command moveToTarget = new FunctionalCommand( - () -> SmartDashboard.putBoolean("Turret/IsStowing", true), - () -> { - setHoodAngle(Constants.Turret.HOOD_STOW_POSITION); - }, - interrupted -> { - }, - - () -> Math.abs( - getHoodAngle() - .minus(Constants.Turret.HOOD_STOW_POSITION) - .getRadians()) < Constants.Turret.HOOD_TOLERANCE.getRadians(), - - this); + double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); // rad/s + double maxSettleTime = Constants.Turret.STOW_PUSH_DOWN_TIME; // seconds, or a separate constant - Command settleDown = new RunCommand( - () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED), // small constant downward speed - this).withTimeout(Constants.Turret.STOW_PUSH_DOWN_TIME); // enough to seat the gear + Command settleDown = Commands.run( + () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED), + this) + // stop when encoder velocity magnitude <= threshold + .until(() -> Math.abs(getHoodVelocity()) <= stopThreshold) + // but no longer than maxSettleTime + .withTimeout(maxSettleTime) + // ensure the servo is stopped when this command completes + .andThen(() -> pitchServo.setSpeed(0.0), this); Command finish = new InstantCommand(() -> { pitchServo.setSpeed(0.0); }); - return Commands.sequence(moveToTarget, settleDown, finish, Commands.idle()) + return Commands.sequence(settleDown, finish, Commands.idle()) .withName("StowHood"); } From 521389dd56e65bc3c3e087f4702d4c97a33ef870 Mon Sep 17 00:00:00 2001 From: David Polcen Date: Thu, 9 Apr 2026 18:30:53 -0500 Subject: [PATCH 3/7] Fixed the settle command logic --- src/main/java/frc/robot/Constants.java | 3 ++ .../frc/robot/subsystems/TurretSubsystem.java | 34 +++++++++++++++---- 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d6e8d43..21492dc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -390,6 +390,9 @@ public static class Turret { public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(5.00); public static final Rotation2d HOOD_STOW_POSITION = Rotation2d.fromDegrees(3.00); + public static final double HOOD_STABLE_TIME = 0.1; + public static final double HOOD_STABLE = 5; + public static final double HOOD_FINISH_VELOCITY = 0.0; public static final double OUTPUT_RANGE_MAX = 1; diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index d84d60d..b4b0f9c 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -21,6 +21,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -28,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Configs; import frc.robot.Constants; import frc.robot.util.turret.TurretSolver; @@ -274,20 +276,38 @@ public Command getAngleCommand( public Command getStowCommand() { - + final Timer settleTimer = new Timer(); double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); // rad/s - double maxSettleTime = Constants.Turret.STOW_PUSH_DOWN_TIME; // seconds, or a separate constant + double maxSettleTime = Constants.Turret.HOOD_STABLE_TIME; // seconds, or a separate constant Command settleDown = Commands.run( () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED), this) // stop when encoder velocity magnitude <= threshold - .until(() -> Math.abs(getHoodVelocity()) <= stopThreshold) - // but no longer than maxSettleTime - .withTimeout(maxSettleTime) + .until(() -> { + boolean within = Math.abs(getHoodVelocity()) <= stopThreshold; + + if (within) { + if (!settleTimer.isRunning()) { + settleTimer.reset(); + settleTimer.start(); + } + //Finish when considered stable + return settleTimer.hasElapsed(maxSettleTime); + } else { + settleTimer.stop(); + settleTimer.reset(); + return false; + } + } + ) // ensure the servo is stopped when this command completes - .andThen(() -> pitchServo.setSpeed(0.0), this); - + .andThen(() -> { + pitchServo.setSpeed(0.0); + settleTimer.stop(); + settleTimer.reset(); + } + ); Command finish = new InstantCommand(() -> { pitchServo.setSpeed(0.0); }); From 4749b4ea89c848c95567c237df88d3e00765c3ea Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Thu, 9 Apr 2026 19:47:50 -0500 Subject: [PATCH 4/7] Run at 50% speed when the shooter button is pressed --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 3 +++ .../frc/robot/subsystems/DriveSubsystem.java | 19 +++++++++++++++++++ 3 files changed, 24 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 21492dc..70740e5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,6 +43,8 @@ public static class Drive { public static final double TARGET_TRANSLATION_RADIUS = 2.0; + public static final double SHOOTING_SPEED_SCALE = 0.5; // Scale factor applied to drive speed while shooting + } public static class ErrorSettings { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a68a1a1..e074442 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -269,6 +269,9 @@ public void configureBindings() { Trigger shootTrigger = new Trigger(() -> driverController.getAButton()); + // Limit drive speed to 50% while the shooting button is held + driveSubsystem.setSpeedScaleSupplier(() -> driverController.getAButton() ? Constants.Operator.Drive.SHOOTING_SPEED_SCALE : 1.0); + Trigger intakeTrigger = new Trigger(() -> driverController.getRightBumperButton()); Trigger solverValid = new Trigger(() -> TurretSolver.solve(driveSubsystem.getPose(), driveSubsystem.getVelocity(), diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 8f0c291..e421cec 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -58,6 +58,9 @@ public class DriveSubsystem extends SubsystemBase { // Feature Flags private boolean useVisionOdometry = true; + // Speed scaling (e.g. 0.5 to limit to 50% while shooting) + private Supplier speedScaleSupplier = () -> 1.0; + public DriveSubsystem() { SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; @@ -190,6 +193,16 @@ public void removeObstaclesSupplier(Supplier> supplier) { obstaclesSuppliers.remove(supplier); } + /** + * Sets a supplier that returns a speed scale factor applied to all drive output. + * A value of 1.0 is full speed; 0.5 limits the robot to 50% of normal speed. + * + * @param supplier Supplier returning a scale factor in the range [0.0, 1.0]. + */ + public void setSpeedScaleSupplier(Supplier supplier) { + this.speedScaleSupplier = (supplier != null) ? supplier : () -> 1.0; + } + // ------------------------------ // POSE ACCESS // ------------------------------ @@ -300,6 +313,12 @@ public void driveWithCompositeRequests(TranslationRequest tReq, RotationRequest ); } + // Apply speed scale (e.g. 50% while shooting) + double scale = MathUtil.clamp(speedScaleSupplier.get(), 0.0, 1.0); + chassisSpeeds.vxMetersPerSecond *= scale; + chassisSpeeds.vyMetersPerSecond *= scale; + chassisSpeeds.omegaRadiansPerSecond *= scale; + // --- Drive: field-oriented unless robot-relative velocity --- if (tReq instanceof TranslationRequest.Velocity vel && !vel.fieldRelative()) { swerveDrive.drive(chassisSpeeds); // robot-relative velocity From c34030c3d5d84fc116d7bea4ad4cf9d1e2fd3b59 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Thu, 9 Apr 2026 20:31:03 -0500 Subject: [PATCH 5/7] Remove unused import Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e074442..11d9496 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; From 7e9c2301f98f8842f6075af1aaa359231e33109f Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Thu, 9 Apr 2026 20:31:28 -0500 Subject: [PATCH 6/7] Remove unused import Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index b4b0f9c..9f09614 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -29,7 +29,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Configs; import frc.robot.Constants; import frc.robot.util.turret.TurretSolver; From 80f2d4a5dde96d0a4ae046504ce7c14c1054661a Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Thu, 9 Apr 2026 20:33:16 -0500 Subject: [PATCH 7/7] Add move to stow position before calling settle down Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- .../java/frc/robot/subsystems/TurretSubsystem.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 9f09614..f56769b 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -274,11 +274,19 @@ public Command getAngleCommand( public Command getStowCommand() { - final Timer settleTimer = new Timer(); double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); // rad/s double maxSettleTime = Constants.Turret.HOOD_STABLE_TIME; // seconds, or a separate constant + Command moveToStow = Commands.run( + () -> setHoodAngle(Constants.Turret.HOOD_STOW_POSITION), + this) + .until(() -> Math.abs( + getHoodAngle() + .minus(Constants.Turret.HOOD_STOW_POSITION) + .getRadians()) < Constants.Turret.HOOD_TOLERANCE.getRadians()) + .finallyDo(interrupted -> stopHoodServo()); + Command settleDown = Commands.run( () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED), this) @@ -311,7 +319,7 @@ public Command getStowCommand() { pitchServo.setSpeed(0.0); }); - return Commands.sequence(settleDown, finish, Commands.idle()) + return Commands.sequence(moveToStow, settleDown, finish, Commands.idle()) .withName("StowHood"); }