From ad0a34572bc57133a8c8c8c9c12468817f121150 Mon Sep 17 00:00:00 2001 From: David Polcen Date: Tue, 7 Apr 2026 18:40:12 -0500 Subject: [PATCH 01/11] 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 02/11] 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 03/11] 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 04/11] 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 05/11] 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 06/11] 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 07/11] 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"); } From 6b66f495880de6fbf80c9f10ff600e60f81993b5 Mon Sep 17 00:00:00 2001 From: CrumblzTheEgg Date: Fri, 10 Apr 2026 10:30:08 -0500 Subject: [PATCH 08/11] fix: remove arbitrary targeting on the stow command --- .../java/frc/robot/subsystems/TurretSubsystem.java | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index f56769b..4e9c995 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -278,15 +278,6 @@ public Command getStowCommand() { 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) @@ -319,7 +310,7 @@ public Command getStowCommand() { pitchServo.setSpeed(0.0); }); - return Commands.sequence(moveToStow, settleDown, finish, Commands.idle()) + return Commands.sequence(settleDown, finish, Commands.idle()) .withName("StowHood"); } From 564befc3dff649a44baca720300764fdcc94ccb5 Mon Sep 17 00:00:00 2001 From: CrumblzTheEgg Date: Fri, 10 Apr 2026 10:34:38 -0500 Subject: [PATCH 09/11] feat: add hood angle reset and remove unessesary constants. --- src/main/java/frc/robot/Constants.java | 3 +-- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 70740e5..f24f2f6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -388,9 +388,8 @@ public static class Turret { public static final boolean PITCH_INVERTED = true; public static final Rotation2d MAX_PITCH = Rotation2d.fromDegrees(45.0); - public static final Rotation2d MIN_PITCH = Rotation2d.fromDegrees(3.00); + public static final Rotation2d MIN_PITCH = Rotation2d.fromDegrees(5.00); 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; diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 4e9c995..f772205 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -308,6 +308,7 @@ public Command getStowCommand() { ); Command finish = new InstantCommand(() -> { pitchServo.setSpeed(0.0); + resetHoodAngle(Constants.Turret.HOOD_START_POSITION); }); return Commands.sequence(settleDown, finish, Commands.idle()) From 853bd7fa846fea617a6ed0954b349da5d4275cfb Mon Sep 17 00:00:00 2001 From: CrumblzTheEgg Date: Fri, 10 Apr 2026 10:41:57 -0500 Subject: [PATCH 10/11] fix: set variable correctly and rename variables so they make more sense --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f24f2f6..06c5e39 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -394,7 +394,7 @@ public static class Turret { 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 HOOD_FINISH_VELOCITY = 0.01; public static final double OUTPUT_RANGE_MAX = 1; public static final double OUTPUT_RANGE_MIN = -1; diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index f772205..cc951f3 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -276,7 +276,7 @@ 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 + double stableTime = 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), @@ -291,7 +291,7 @@ public Command getStowCommand() { settleTimer.start(); } //Finish when considered stable - return settleTimer.hasElapsed(maxSettleTime); + return settleTimer.hasElapsed(stableTime); } else { settleTimer.stop(); settleTimer.reset(); From 8a1c7dcbdc819a728fda7e2ba4cf6e08aec4d77d Mon Sep 17 00:00:00 2001 From: N/A Date: Fri, 10 Apr 2026 19:42:43 -0500 Subject: [PATCH 11/11] refactor: rework base hood angle to account for hardstop --- src/main/java/frc/robot/Constants.java | 11 +- src/main/java/frc/robot/RobotContainer.java | 136 +----------------- .../frc/robot/subsystems/TurretSubsystem.java | 8 +- 3 files changed, 20 insertions(+), 135 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3c3ec25..1d68ebd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -409,8 +409,8 @@ public static class Turret { public static final double TURN_TABLE_RATIO = 24.0 / 200.0; public static final double ENCODER_FACTOR = (TURRET_GEAR_REDUCTION) / (2.0 * Math.PI * TURN_TABLE_RATIO); - public static final double STOW_PUSH_DOWN_SPEED = -0.3; // percent of max speed - public static final double STOW_PUSH_DOWN_TIME = 0.5; // seconds + public static final double STOW_PUSH_DOWN_SPEED = 0.6; // percent of max speed + public static final double STOW_PUSH_DOWN_TIME = 0.1; // seconds public static final double PITCH_GEAR_RATIO = (26.0 / 447.2); public static final double PITCH_ENCODER_FACTOR = PITCH_GEAR_RATIO * (2.0 * Math.PI); @@ -418,13 +418,14 @@ public static class Turret { public static final boolean PITCH_INVERTED = true; public static final Rotation2d MAX_PITCH = Rotation2d.fromDegrees(45.0); - public static final Rotation2d MIN_PITCH = Rotation2d.fromDegrees(5.00); - public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(5.00); + public static final Rotation2d MIN_PITCH = Rotation2d.fromDegrees(8.00); + public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(8.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.01; + public static final double HOOD_FINISH_VELOCITY = 0.1; + public static final Rotation2d HOOD_FAR_ANGLE = Rotation2d.fromDegrees(-50.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 d46a91a..94a3ec6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -269,135 +269,6 @@ public void updateFieldObjects() { public void configureBindings() { - // Driver controls - - Trigger stowTrigger = new Trigger( - () -> driverController.getBButton() || SwerveUtil.willRobotEnterRegion(driveSubsystem.getPose(), - driveSubsystem.getVelocity(), Constants.Field.TRENCH_REGION, Constants.Drive.HOOD_STOW_LOOKAHEAD_TIME)); - stowTrigger.and(() -> !FieldUtil.isAutonomous()).whileTrue( - turretSubsystem.getStowCommand()); - - 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(), - targetingSupplier.get(), Constants.Turret.SOLVER_CONFIG).isValid()); - - solverValid.onTrue( - Commands.runOnce(() -> SmartDashboard.putBoolean("Turret/SolverValid", true)).ignoringDisable(true)).onFalse( - Commands.runOnce(() -> SmartDashboard.putBoolean("Turret/SolverValid", false)).ignoringDisable(true)); - - /* Shooter runs while button held */ - shootTrigger.whileTrue( - shooterSubsystem.getTargetCommand( - targetingSupplier, - driveSubsystem::getPose, - driveSubsystem::getVelocity)); - - shootTrigger.whileTrue( - turretSubsystem.getTargetCommand( - targetingSupplier, - driveSubsystem::getPose, - driveSubsystem::getVelocity - ) - ); - - /* Transfer runs ONLY while button AND solver valid */ - shootTrigger - .whileTrue( - new WaitCommand(Constants.Transfer.LOAD_DELAY).andThen( - transferSubsystem.getLoadCommand() - )); - - - /* Intake pivot runs while button held */ - intakeTrigger.whileTrue( - intakePivot.deployIntakeCommand()); - - /* Intake roller runs while button held */ - intakeTrigger.whileTrue( - intakeRoller.getIntakeCommand()); - - /* Stop shooter on button release */ - shootTrigger.onFalse( - Commands.parallel( - transferSubsystem.getStopCommand(), - shooterSubsystem.getStopCommand())); - - /* Stop intake on button release */ - intakeTrigger.onFalse( - Commands.parallel( - intakePivot.raiseIntakeCommand(), - intakeRoller.getStopCommand())); - - // ============================== - // Turret Offset Adjustment (POV Left / Right) - // ============================== - - // POV Left (225°–315°) : Decrease turret offset - new Trigger(() -> { - int pov = operatorController.getPOV(); - return pov >= 225 && pov <= 315; - }) - .whileTrue( - turretOffsetDegrees.getHeldIntervalCommand(-Constants.Operator.ErrorSettings.TURRET_OFFSET_INCREASE, - Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); - - // POV Right (45°–135°) : Increase turret offset - new Trigger(() -> { - int pov = operatorController.getPOV(); - return pov >= 45 && pov <= 135; - }) - .whileTrue( - turretOffsetDegrees.getHeldIntervalCommand(Constants.Operator.ErrorSettings.TURRET_OFFSET_INCREASE, - Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); - - // ============================== - // Hood Offset Adjustment (D-Pad Up / D-Pad Down) - // ============================== - - // D-Pad Up : Increase hood offset - new Trigger(() -> { - int pov = operatorController.getPOV(); - return pov >= 315 || (pov >= 0 && pov <= 45); - }) - .whileTrue( - hoodOffsetDegrees.getHeldIntervalCommand(Constants.Operator.ErrorSettings.HOOD_OFFSET_INCREASE, - Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); - - // D-Pad Down : Decrease hood offset - new Trigger(() -> { - int pov = operatorController.getPOV(); - return pov >= 135 && pov <= 225; - }) - .whileTrue( - hoodOffsetDegrees.getHeldIntervalCommand(-Constants.Operator.ErrorSettings.HOOD_OFFSET_INCREASE, - Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); - - // ============================== - // Shooter Percent Adjustment (Left Bumper / Right Bumper) - // ============================== - - // Left Bumper : Decrease shooter percent - new Trigger(() -> { - return operatorController.getLeftBumperButton(); - }) - .whileTrue( - shooterPercent.getHeldIntervalCommand(-Constants.Operator.ErrorSettings.SHOOTER_PERCENT_INCREASE, - Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); - - // Right Bumper : Increase shooter percent - new Trigger(() -> { - return operatorController.getRightBumperButton(); - }) - .whileTrue( - shooterPercent.getHeldIntervalCommand(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_INCREASE, - Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME)); - new Trigger( () -> operatorController.getAButton()).onTrue( new InstantCommand(() -> selectedFixedTarget = FixedTarget.A)); @@ -529,6 +400,13 @@ public void configureBindings() { targetingSupplier, driveSubsystem::getPose, driveSubsystem::getVelocity)); + + /* Turret runs while button held */ + shootTrigger.whileTrue( + turretSubsystem.getTargetCommand( + targetingSupplier, + driveSubsystem::getPose, + driveSubsystem::getVelocity)); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6a55784..b62c619 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -346,7 +346,10 @@ public Command getStowCommand() { double stableTime = 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), + () -> { + pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED); + SmartDashboard.putBoolean("Hood Pushing", true); + }, this) // stop when encoder velocity magnitude <= threshold .until(() -> { @@ -365,6 +368,8 @@ public Command getStowCommand() { return false; } } + ).until( + () -> getHoodAngle().getRadians() < Constants.Turret.HOOD_FAR_ANGLE.getRadians() ) // ensure the servo is stopped when this command completes .andThen(() -> { @@ -374,6 +379,7 @@ public Command getStowCommand() { } ); Command finish = new InstantCommand(() -> { + SmartDashboard.putBoolean("Hood Pushing", false); pitchServo.setSpeed(0.0); resetHoodAngle(Constants.Turret.HOOD_START_POSITION); });