Merge pull request #23 from StuyPulse/se/control-changes

Se/control changes
StuyPulse · Aug 1, 2024 · 86dad6f · 86dad6f
2 parents acd3e05 + 82bb153
commit 86dad6f
Show file tree

Hide file tree

Showing 33 changed files with 465 additions and 295 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -2,12 +2,11 @@
 
 import com.ctre.phoenix6.Utils;
 import com.stuypulse.robot.commands.BuzzController;
-import com.stuypulse.robot.commands.arm.ArmSetShootHeightToHigh;
-import com.stuypulse.robot.commands.arm.ArmSetShootHeightToLow;
 import com.stuypulse.robot.commands.arm.ArmToAmp;
 import com.stuypulse.robot.commands.arm.ArmToClimbing;
 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.ArmToStow;
@@ -24,36 +23,43 @@
 import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker;
 import com.stuypulse.robot.commands.shooter.ShooterSetRPM;
 import com.stuypulse.robot.commands.shooter.ShooterStop;
+import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToClimb;
 import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
 import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp;
+import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLobFerry;
 import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry;
-import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerHigh;
-import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerLow;
-import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerry;
-import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerryManual;
+import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualLobFerry;
+import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker;
+import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLobFerry;
+import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLobFerryManual;
+import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLowFerry;
+import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLowFerryManual;
 import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndScoreSpeaker;
 import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote;
 import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Driver;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 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.ShooterLobFerryInterpolation;
 import com.stuypulse.robot.util.ShooterSpeeds;
 import com.stuypulse.robot.subsystems.arm.Arm;
 import com.stuypulse.robot.subsystems.intake.Intake;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -112,7 +118,7 @@ private void configureButtonBindings() {
     }
 
     private void configureDriverBindings() {
-        driver.getRightMenuButton().onTrue(new SwerveDriveSeedFieldRelative());
+        driver.getDPadUp().onTrue(new SwerveDriveSeedFieldRelative());
 
         // intake field relative
         driver.getRightTriggerButton()
@@ -135,49 +141,89 @@ private void configureDriverBindings() {
             .whileTrue(new IntakeDeacquire())
             .onFalse(new IntakeStop());
 
-        driver.getDPadUp().onTrue(new ArmSetShootHeightToHigh());
-        driver.getDPadDown().onTrue(new ArmSetShootHeightToLow());
-
         // speaker align and score 
+        // score amp
         driver.getRightBumper()
-            .whileTrue(new SwerveDriveDriveAndScoreSpeaker(driver));
+            .whileTrue(new ConditionalCommand(
+                new ArmWaitUntilAtTarget()
+                    .withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
+                    .andThen(new ShooterScoreAmp()),
+                new SwerveDriveDriveAlignedSpeaker(driver)
+                    .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER))
+                        .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
+                                .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
+                        .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker()))
+                        .andThen(new ShooterFeederShoot())
+                    ),
+                () -> Arm.getInstance().getState() == Arm.State.AMP))
+            .onFalse(new ConditionalCommand(
+                new ShooterFeederStop(), 
+                new ShooterStop(), 
+                () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
         // ferry align and shoot
         // move to back of controller
-        driver.getRightStickButton()
-            .whileTrue(new SwerveDriveDriveAndFerry(driver));
+        driver.getDPadRight()
+            .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver)
+                    .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
+                        .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
+                                .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
+                        .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry()))
+                        .andThen(new ShooterFeederShoot())
+                    )
+            )
+            .onFalse(new ConditionalCommand(
+                new ShooterFeederStop(), 
+                new ShooterStop(), 
+                () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
+
+        driver.getDPadDown()
+            .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver)
+                    .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
+                        .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
+                                .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
+                        .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry()))
+                        .andThen(new ShooterFeederShoot())
+                    )
+            )
+            .onFalse(new ConditionalCommand(
+                new ShooterFeederStop(), 
+                new ShooterStop(), 
+                () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
         // arm to amp and alignment
         driver.getLeftBumper()
             .onTrue(new ArmToAmp())
             .onTrue(new SwerveDriveDriveAlignedAmp(driver));
 
         // manual speaker at subwoofer
-        // score amp
         // rebind to a button on the back later
-        driver.getLeftStickButton()
-            .whileTrue(new ConditionalCommand(
-                new ShooterScoreAmp(), 
-                new ArmToSubwooferShot()
-                    .andThen(new ShooterScoreSpeaker()), 
-                () -> Arm.getInstance().getState() == Arm.State.AMP))
+        driver.getRightMenuButton()
+            .whileTrue(new ArmToSubwooferShot()
+                        .andThen(new ArmWaitUntilAtTarget())
+                        .andThen(new ShooterScoreSpeaker()))
             .onFalse(new ConditionalCommand(
                 new ShooterFeederStop(), 
                 new ShooterStop(), 
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
         // manual ferry
         driver.getTopButton()
-            .whileTrue(new SwerveDriveDriveAndFerryManual(driver));
-
-        driver.getRightButton()
-            .onTrue(new ArmToPreClimb());
-        driver.getRightButton()
-            .debounce(Settings.Driver.TIME_UNTIL_HOLD)
-            .whileTrue(new SwerveDriveDriveToChain());
+            .whileTrue(new SwerveDriveDriveAndLobFerryManual(driver))
+            .onFalse(new ConditionalCommand(
+                new ShooterFeederStop(), 
+                new ShooterStop(), 
+                () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
+        driver.getLeftButton()
+            .whileTrue(new SwerveDriveDriveAndLowFerryManual(driver))
+            .onFalse(new ConditionalCommand(
+                new ShooterFeederStop(), 
+                new ShooterStop(), 
+                () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
-        driver.getBottomButton().whileTrue(new SwerveDriveDriveToChain());
-        driver.getLeftMenuButton().onTrue(new ArmToClimbing());
+        // climbing
+        driver.getRightButton().onTrue(new ArmToPreClimb());
+        driver.getBottomButton().onTrue(new ArmToClimbing());
     }
 
     private void configureOperatorBindings() {

diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java
@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+public class ArmToLobFerry extends ArmSetState{
+
+    public ArmToLobFerry(){
+        super(Arm.State.LOB_FERRY);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java
@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+public class ArmToLowFerry extends ArmSetState{
+
+    public ArmToLowFerry(){
+        super(Arm.State.LOW_FERRY);
+    }
+}
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,5 +1,6 @@
 package com.stuypulse.robot.commands.shooter;
 
+import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -9,7 +10,7 @@ public class ShooterFerry extends SequentialCommandGroup {
     public ShooterFerry() {
         addCommands(
             new ShooterSetRPM(Shooter.getInstance()::getFerrySpeeds),
-            new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)),
+            new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET),
             new ShooterFeederShoot()
         );
     }

diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLobFerryManual.java
@@ -0,0 +1,28 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.util.ShooterLobFerryInterpolation;
+import com.stuypulse.robot.util.ShooterSpeeds;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class ShooterLobFerryManual extends SequentialCommandGroup {
+
+    public ShooterLobFerryManual() {
+        addCommands(
+            new ShooterSetRPM(new ShooterSpeeds(ShooterLobFerryInterpolation.getRPM(getFerryDistance()))),
+            new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)),
+            new ShooterFeederShoot()
+        );
+    }
+
+    private double getFerryDistance() {
+        return Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose()));
+    }
+
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLowFerryManual.java
@@ -0,0 +1,28 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.util.ShooterLowFerryInterpolation;
+import com.stuypulse.robot.util.ShooterSpeeds;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class ShooterLowFerryManual extends SequentialCommandGroup {
+
+    public ShooterLowFerryManual() {
+        addCommands(
+            new ShooterSetRPM(new ShooterSpeeds(ShooterLowFerryInterpolation.getRPM(getFerryDistance()))),
+            new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)),
+            new ShooterFeederShoot()
+        );
+    }
+
+    private double getFerryDistance() {
+        return Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose()));
+    }
+
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java
@@ -1,5 +1,6 @@
 package com.stuypulse.robot.commands.shooter;
 
+import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
 import com.stuypulse.robot.constants.Settings;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;

diff --git a/...ava/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java b/...ava/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java
@@ -14,21 +14,13 @@ public SwerveDriveDriveAlignedLobFerry(Gamepad driver) {
         super(driver);
     }
 
-    private Translation2d getAmpCornerPose() {
-        Translation2d targetPose = Robot.isBlue()
-            ? new Translation2d(0.0, Field.WIDTH - 1.5)
-            : new Translation2d(0.0, 1.5);
-
-        return targetPose;
-    }
-
     @Override
     protected Rotation2d getTargetAngle() {
-        return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180));
+        return SwerveDrive.getInstance().getPose().getTranslation().minus(Field.getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180));
     }
 
     @Override
     protected double getDistanceToTarget() {
-        return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose());
+        return SwerveDrive.getInstance().getPose().getTranslation().getDistance(Field.getAmpCornerPose());
     }
 }