Merge pull request #20 from StuyPulse/se/arm-overrides

Se/arm overrides
StuyPulse · Jul 26, 2024 · 5862fe1 · 5862fe1
2 parents cd287a4 + 1257c63
commit 5862fe1
Show file tree

Hide file tree

Showing 18 changed files with 247 additions and 56 deletions.
diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java
@@ -37,7 +37,7 @@ public void robotInit() {
 
     @Override
     public void robotPeriodic() {
-        if (Arm.getInstance().getState() == Arm.State.FEED 
+        if (Arm.getInstance().getActualState() == Arm.State.FEED 
             && Arm.getInstance().atTarget() 
             && !Shooter.getInstance().hasNote()
             && Intake.getInstance().hasNote()

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -2,12 +2,17 @@
 
 import com.ctre.phoenix6.Utils;
 import com.stuypulse.robot.commands.BuzzController;
+import com.stuypulse.robot.commands.arm.ArmDisableOverride;
+import com.stuypulse.robot.commands.arm.ArmEnableOverride;
 import com.stuypulse.robot.commands.arm.ArmToAmp;
 import com.stuypulse.robot.commands.arm.ArmToFeed;
+import com.stuypulse.robot.commands.arm.ArmToFerry;
 import com.stuypulse.robot.commands.arm.ArmToLobFerry;
 import com.stuypulse.robot.commands.arm.ArmToLowFerry;
 import com.stuypulse.robot.commands.arm.ArmToPreClimb;
+import com.stuypulse.robot.commands.arm.ArmToSpeaker;
 import com.stuypulse.robot.commands.arm.ArmToSpeakerHigh;
+import com.stuypulse.robot.commands.arm.ArmToSpeaker;
 import com.stuypulse.robot.commands.arm.ArmToSpeakerLow;
 import com.stuypulse.robot.commands.arm.ArmToStow;
 import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
@@ -19,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;
@@ -35,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;
 
@@ -96,6 +103,7 @@ private void configureDefaultCommands() {
     private void configureButtonBindings() {
         driver.getRightBumper().whileTrue(new SwerveDriveXMode());
 
+        driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative());
         driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver));
 
         driver.getLeftTriggerButton()
@@ -111,26 +119,37 @@ 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 ConditionalCommand(
-                new ArmToSpeakerHigh(), 
-                new ArmToSpeakerLow(), 
-                () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW));
-
-        driver.getLeftButton().onTrue(new ArmToAmp());
+            .onTrue(new ArmToSpeaker());
+
+        driver.getTopButton()
+            .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME)
+            .onTrue(new ArmEnableOverride())
+            .onFalse(new ArmDisableOverride());
+
+        driver.getLeftButton()
+            .onTrue(new ArmToAmp());
+
+        driver.getLeftButton()
+            .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME)
+            .onTrue(new ArmEnableOverride())
+            .onFalse(new ArmDisableOverride());
 
         driver.getRightButton()
-            .onTrue(new ConditionalCommand(
-                new ArmToLobFerry(), 
-                new ArmToLowFerry(), 
-                () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY));
+            .whileTrue(new ArmToFerry());
+
+        driver.getRightButton()
+            .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME)
+            .onTrue(new ArmEnableOverride())
+            .onFalse(new ArmDisableOverride());
 
         driver.getBottomButton().onTrue(new ArmToFeed());
 
-        driver.getDPadUp().onTrue(new ArmToPreClimb());
-        driver.getDPadDown().onTrue(new ArmToStow());
+        driver.getDPadUp().whileTrue(new ArmToPreClimb());
+        driver.getDPadDown().whileTrue(new ArmToStow());
 
     }
 

diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java
@@ -0,0 +1,19 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ArmDisableOverride extends InstantCommand{
+
+    private final Arm arm;
+
+    public ArmDisableOverride() {
+        this.arm = Arm.getInstance();
+    }
+
+    @Override
+    public void initialize() {
+        arm.setOverriding(false);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java
@@ -0,0 +1,19 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ArmEnableOverride extends InstantCommand{
+
+    private final Arm arm;
+
+    public ArmEnableOverride() {
+        this.arm = Arm.getInstance();
+    }
+
+    @Override
+    public void initialize() {
+        arm.setOverriding(true);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java
@@ -1,7 +1,6 @@
 package com.stuypulse.robot.commands.arm;
 
 import com.stuypulse.robot.subsystems.arm.Arm;
-
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class ArmSetState extends InstantCommand{
@@ -17,6 +16,6 @@ public ArmSetState(Arm.State state) {
 
     @Override
     public void initialize() {
-        arm.setState(state);
+        arm.setRequestedState(state);
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java
@@ -0,0 +1,33 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.arm.Arm;
+import com.stuypulse.stuylib.util.StopWatch;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ArmToFerry extends InstantCommand{
+
+    private final Arm arm;
+    private final StopWatch stopWatch;
+    private double lastClick;
+
+    public ArmToFerry() {
+        arm = Arm.getInstance();
+        stopWatch = new StopWatch();
+        lastClick = 0;
+        addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) {
+            arm.setRequestedState(Arm.State.LOB_FERRY);
+        }
+        else {
+            arm.setRequestedState(Arm.State.LOW_FERRY);
+        }
+        lastClick = stopWatch.getTime();
+    }
+
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java
@@ -0,0 +1,33 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.arm.Arm;
+import com.stuypulse.stuylib.util.StopWatch;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ArmToSpeaker extends InstantCommand{
+
+    private final Arm arm;
+    private final StopWatch stopWatch;
+    private double lastClick;
+
+    public ArmToSpeaker() {
+        arm = Arm.getInstance();
+        stopWatch = new StopWatch();
+        lastClick = 0;
+        addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) {
+            arm.setRequestedState(Arm.State.SPEAKER_HIGH);
+        }
+        else {
+            arm.setRequestedState(Arm.State.SPEAKER_LOW);
+        }
+        lastClick = stopWatch.getTime();
+    }
+
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java
@@ -12,7 +12,7 @@ public class ShooterAutoShoot extends InstantCommand{
 
     @Override
     public void initialize() {
-        switch (Arm.getInstance().getState()) {
+        switch (Arm.getInstance().getActualState()) {
             case AMP:
                 CommandScheduler.getInstance().schedule(new ShooterScoreAmp());
                 break;

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/commands/swerve/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java
@@ -22,7 +22,7 @@ public SwerveDriveAutoAlignment(Gamepad driver) {
 
     @Override
     public void initialize() {
-        switch (Arm.getInstance().getState()) {
+        switch (Arm.getInstance().getActualState()) {
             case AMP:
                 CommandScheduler.getInstance().schedule(new SwerveDriveDriveAmpAligned(driver));
                 break;

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -114,8 +114,10 @@ public interface Shooter {
         double FEEDER_DEAQUIRE_SPEED = 0.5;
         double FEEDER_SHOOT_SPEED = 0.25;
 
+        boolean ALWAYS_KEEP_AT_SPEED = false;
+
         double TARGET_RPM_THRESHOLD = 250;
-        double MAX_WAIT_TO_REACH_TARGET = 1.5;
+        double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0;
 
         ShooterSpeeds SPEAKER = new ShooterSpeeds(
             new SmartNumber("Shooter/Speaker RPM", 4875), 
@@ -325,6 +327,9 @@ public interface Rotation {
     }
 
     public interface Driver {
+        double HOLD_TO_OVERRIDE_TIME = 0.55;
+        double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5;
+
         public interface Drive {
             SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03);
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java
@@ -1,5 +1,7 @@
 package com.stuypulse.robot.subsystems.arm;
 
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -28,26 +30,44 @@ public enum State {
         RESETTING
     }
 
-    protected State state;
+    protected State requestedState;
+    protected State actualState;
+    protected boolean overriding;
 
     protected Arm() {
-        state = State.RESETTING;
+        requestedState = State.RESETTING;
+        actualState = State.RESETTING;
+        overriding = false;
+    }
+
+    public void setRequestedState(State state) {
+        this.requestedState = state;
+    }
+
+    public State getRequestedState() {
+        return this.requestedState;
+    }
+
+    public State getActualState() {
+        return this.actualState;
     }
 
-    public void setState(State state) {
-        if (this.state != State.RESETTING) {
-            this.state = state;
-        }
+    public void setOverriding(boolean overriding) {
+        this.overriding = overriding;
     }
 
-    public State getState() {
-        return this.state;
+    public boolean isOverriding() {
+        return this.overriding;
     }
 
     public abstract boolean atTarget();
 
+    public abstract boolean shouldReturnToFeed();
+
     @Override
     public void periodic() {
-        SmartDashboard.putString("Arm/State", getState().toString());
+        SmartDashboard.putBoolean("Arm/Is Overriding", overriding);
+        SmartDashboard.putString("Arm/Requested State", getRequestedState().toString());
+        SmartDashboard.putString("Arm/Actual State", getActualState().toString());
     }
 }