settings to always hold shooter speed

StuyPulse · Jul 26, 2024 · 1257c63 · 1257c63
1 parent 3b46640
commit 1257c63
Show file tree

Hide file tree

Showing 7 changed files with 45 additions and 20 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<ShooterSpeeds> speeds;
 
-    public ShooterSetRPM(ShooterSpeeds speeds) {
+    public ShooterSetRPM(Supplier<ShooterSpeeds> 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());
     }
 
 }
diff --git 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
@@ -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
@@ -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());