diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 87eacde..06c5e39 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 { @@ -386,9 +388,13 @@ 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; + + 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; @@ -460,6 +466,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..11d9496 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -268,6 +268,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(), @@ -284,10 +287,21 @@ 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( - transferSubsystem.getLoadCommand()); + new WaitCommand(Constants.Transfer.LOAD_DELAY).andThen( + transferSubsystem.getLoadCommand() + )); + /* Intake pivot runs while button held */ intakeTrigger.whileTrue( @@ -515,10 +529,7 @@ public void scheduleTeleOp() { manualRotationVelocityDirective, null, null); driveSubsystem.setDefaultCommand(manualDriveCommand); - turretSubsystem.setDefaultCommand(turretSubsystem.getTargetCommand( - targetingSupplier, - driveSubsystem::getPose, - driveSubsystem::getVelocity)); + turretSubsystem.setDefaultCommand(turretSubsystem.getStowCommand()); } 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 diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index d79fbb3..cc951f3 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; @@ -168,6 +169,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 +274,44 @@ 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); - - 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 - + final Timer settleTimer = new Timer(); + double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); // rad/s + 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), + this) + // stop when encoder velocity magnitude <= threshold + .until(() -> { + boolean within = Math.abs(getHoodVelocity()) <= stopThreshold; + + if (within) { + if (!settleTimer.isRunning()) { + settleTimer.reset(); + settleTimer.start(); + } + //Finish when considered stable + return settleTimer.hasElapsed(stableTime); + } else { + settleTimer.stop(); + settleTimer.reset(); + return false; + } + } + ) + // ensure the servo is stopped when this command completes + .andThen(() -> { + pitchServo.setSpeed(0.0); + settleTimer.stop(); + settleTimer.reset(); + } + ); Command finish = new InstantCommand(() -> { pitchServo.setSpeed(0.0); + resetHoodAngle(Constants.Turret.HOOD_START_POSITION); }); - return Commands.sequence(moveToTarget, settleDown, finish, Commands.idle()) + return Commands.sequence(settleDown, finish, Commands.idle()) .withName("StowHood"); }