diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6f8a677..4808b90 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,6 +45,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 Misc { @@ -407,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); @@ -416,9 +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(3.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 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.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; @@ -521,6 +528,7 @@ public static class Transfer { public static final boolean FEEDER_INVERTED = true; + public static final double LOAD_DELAY = 0.1; //Time between shooter and transfer being triggered, shooter should always activate first public static final double KICKER_LOAD_POWER = 0.25; public static final double FEEDER_LOAD_POWER = 0.5; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 45dd6ab..201aa90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -267,6 +267,26 @@ public void updateFieldObjects() { public void configureBindings() { + new Trigger( + () -> operatorController.getAButton()).onTrue( + new InstantCommand(() -> selectedFixedTarget = FixedTarget.A)); + + new Trigger( + () -> operatorController.getBButton()).onTrue( + new InstantCommand(() -> selectedFixedTarget = FixedTarget.B)); + + new Trigger( + () -> operatorController.getXButton()).onTrue( + new InstantCommand(() -> xHeld = true)) + .onFalse( + new InstantCommand(() -> xHeld = false)); + + new Trigger( + () -> operatorController.getYButton()).onTrue( + new InstantCommand(() -> yHeld = true)) + .onFalse( + new InstantCommand(() -> yHeld = false)); + targetingSupplier = () -> { Translation2d robotPosition = driveSubsystem.getPose().getTranslation(); @@ -379,6 +399,13 @@ public void configureBindings() { driveSubsystem::getPose, driveSubsystem::getVelocity)); + /* Turret runs while button held */ + shootTrigger.whileTrue( + turretSubsystem.getTargetCommand( + targetingSupplier, + driveSubsystem::getPose, + driveSubsystem::getVelocity)); + /* Transfer runs ONLY while button AND solver valid */ shootTrigger.whileTrue( transferSubsystem.getLoadCommand()); @@ -539,10 +566,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 ca39c7d..de51675 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 1c1a2b7..b62c619 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -22,6 +22,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; @@ -194,6 +195,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 */ @@ -336,36 +341,50 @@ public Command getAngleCommand( public Command getStowCommand() { - Command moveToTarget = new FunctionalCommand( - () -> SmartDashboard.putBoolean("Turret/IsStowing", true), + 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( () -> { - setHoodAngle(Constants.Turret.HOOD_STOW_POSITION); - }, - - interrupted -> { + pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED); + SmartDashboard.putBoolean("Hood Pushing", true); }, - - () -> 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 - + 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; + } + } + ).until( + () -> getHoodAngle().getRadians() < Constants.Turret.HOOD_FAR_ANGLE.getRadians() + ) + // ensure the servo is stopped when this command completes + .andThen(() -> { + pitchServo.setSpeed(0.0); + settleTimer.stop(); + settleTimer.reset(); + } + ); Command finish = new InstantCommand(() -> { + SmartDashboard.putBoolean("Hood Pushing", false); 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"); }