diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1ce6980e..b41d3163 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,7 @@ import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.swerve.SwerveDriveAutoAlignment; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry; @@ -40,6 +41,7 @@ import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; @@ -117,7 +119,8 @@ private void configureButtonBindings() { .whileTrue(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .andThen(new ShooterAutoShoot()) ) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ShooterSetRPM(new ShooterSpeeds()).onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); driver.getTopButton() .onTrue(new ArmToSpeaker()); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java index e2aeee4b..ecd37b11 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java @@ -1,13 +1,14 @@ package com.stuypulse.robot.commands.shooter; import com.stuypulse.robot.constants.Settings; - +import com.stuypulse.robot.subsystems.shooter.Shooter; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class ShooterFerry extends SequentialCommandGroup { public ShooterFerry() { addCommands( + new ShooterSetRPM(Shooter.getInstance()::getFerrySpeeds), new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)), new ShooterFeederShoot() ); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java index 61931553..76b2d342 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java @@ -8,6 +8,7 @@ public class ShooterScoreSpeaker extends SequentialCommandGroup { public ShooterScoreSpeaker() { addCommands( + new ShooterSetRPM(Settings.Shooter.SPEAKER), new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET), new ShooterFeederShoot() ); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java index bfeab4fb..31928d8e 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java @@ -1,5 +1,7 @@ package com.stuypulse.robot.commands.shooter; +import java.util.function.Supplier; + import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.util.ShooterSpeeds; @@ -8,19 +10,21 @@ public class ShooterSetRPM extends InstantCommand { private final Shooter shooter; - private final ShooterSpeeds speeds; + private final Supplier speeds; - public ShooterSetRPM(ShooterSpeeds speeds) { + public ShooterSetRPM(Supplier speeds) { shooter = Shooter.getInstance(); - this.speeds = speeds; - addRequirements(shooter); } + public ShooterSetRPM(ShooterSpeeds speeds) { + this(() -> speeds); + } + @Override public void initialize() { - shooter.setTargetSpeeds(speeds); + shooter.setTargetSpeeds(speeds.get()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b5d3ec0d..88658606 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -114,10 +114,10 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 0.25; - double TARGET_RPM_THRESHOLD = 250; - double MAX_WAIT_TO_REACH_TARGET = 1.5; + boolean ALWAYS_KEEP_AT_SPEED = false; - boolean ALWAYS_KEEP_AT_SPEED = true; + double TARGET_RPM_THRESHOLD = 250; + double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0; ShooterSpeeds SPEAKER = new ShooterSpeeds( new SmartNumber("Shooter/Speaker RPM", 4875), diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index b324e3d3..41f6b74d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -22,8 +22,8 @@ public static Shooter getInstance() { private final SmartNumber rightTargetRPM; public Shooter() { - leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.SPEAKER.getLeftRPM()); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.SPEAKER.getRightRPM()); + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0); } public void setTargetSpeeds(ShooterSpeeds speeds) { @@ -47,6 +47,8 @@ public void stop() { public abstract boolean atTargetSpeeds(); + public abstract ShooterSpeeds getFerrySpeeds(); + public abstract void feederIntake(); public abstract void feederDeacquire(); public abstract void feederShoot(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index e64ec59e..59854b2e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -132,7 +132,8 @@ public boolean noteShot() { return getLeftTargetRPM() > 0 && getRightTargetRPM() > 0 && hasNote.get() == false; } - private ShooterSpeeds getFerrySpeeds() { + @Override + public ShooterSpeeds getFerrySpeeds() { Translation2d ferryZone = Robot.isBlue() ? new Translation2d(0, Field.WIDTH - 1.5) : new Translation2d(0, 1.5); @@ -155,18 +156,31 @@ public void periodic () { Arm.State armState = Arm.getInstance().getActualState(); - if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { - setTargetSpeeds(Settings.Shooter.SPEAKER); + if (Settings.Shooter.ALWAYS_KEEP_AT_SPEED) { + if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { + setTargetSpeeds(Settings.Shooter.SPEAKER); + } + else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { + setTargetSpeeds(getFerrySpeeds()); + } + else { + setTargetSpeeds(Settings.Shooter.SPEAKER); + } } - else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { - setTargetSpeeds(getFerrySpeeds()); + + if (getLeftTargetRPM() == 0) { + leftMotor.set(0); } else { - setTargetSpeeds(Settings.Shooter.SPEAKER); + setLeftShooterRPM(getLeftTargetRPM()); } - setLeftShooterRPM(getLeftTargetRPM()); - setRightShooterRPM(getRightTargetRPM()); + if (getRightTargetRPM() == 0) { + rightMotor.set(0); + } + else { + setRightShooterRPM(getRightTargetRPM()); + } SmartDashboard.putNumber("Shooter/Feeder Speed", feederMotor.get());