Merge pull request #18 from StuyPulse/se/amp-alignment

Se/amp alignment
StuyPulse · Jul 21, 2024 · 1bc2430 · 1bc2430
2 parents 1e92b66 + 30cd0a9
commit 1bc2430
Show file tree

Hide file tree

Showing 4 changed files with 77 additions and 24 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -17,6 +17,7 @@
 import com.stuypulse.robot.commands.intake.IntakeStop;
 import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
 import com.stuypulse.robot.commands.shooter.ShooterAutoShoot;
+import com.stuypulse.robot.commands.swerve.SwerveDriveAutoAlignment;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh;
@@ -91,33 +92,15 @@ private void configureDefaultCommands() {
     /***************/
 
     private void configureButtonBindings() {
-        driver.getRightMenuButton().whileTrue(new SwerveDriveXMode());
-        driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative());
+        driver.getRightBumper().whileTrue(new SwerveDriveXMode());
+
+        driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver));
 
         driver.getLeftTriggerButton()
             .whileTrue(new IntakeAcquire()
                 .andThen(new BuzzController(driver))
             );
 
-        // driver.getLeftBumper()
-        //     .whileTrue(new WaitUntilCommand(() -> !shooter.hasNote())
-        //     .andThen(new ArmToFeed()
-        //         .andThen(new WaitUntilCommand(intake::hasNote).alongWith(new ArmWaitUntilAtTarget()))
-        //         .andThen(new ShooterAcquireFromIntake())
-        //         .andThen(new BuzzController(driver))
-        //     )
-        // );
-
-        // driver.getLeftTriggerButton()
-        //     .whileTrue(new ArmToFeed().onlyIf(() -> !shooter.hasNote()))
-        //     .whileTrue(new IntakeAcquire()
-        //         .andThen(new BuzzController(driver))
-        //         .andThen(new WaitUntilCommand(() -> Arm.getInstance().getState() == Arm.State.FEED))
-        //         .andThen(new ArmWaitUntilAtTarget())
-        //         .andThen(new ShooterAcquireFromIntake().onlyIf(() -> !shooter.hasNote()))
-        //         .andThen(new BuzzController(driver)))
-        //     .onFalse(new IntakeStop());
-
         driver.getLeftBumper()
             .whileTrue(new IntakeDeacquire())
             .onFalse(new IntakeStop());
@@ -127,9 +110,6 @@ private void configureButtonBindings() {
                 .andThen(new ShooterAutoShoot())
             );
 
-        driver.getRightBumper()
-            .onTrue(new SwerveDriveDriveAlignedSpeakerLow(driver));
-
         driver.getTopButton()
             .onTrue(new ConditionalCommand(
                 new ArmToSpeakerHigh(), 

diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java
@@ -0,0 +1,46 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.commands.BuzzController;
+import com.stuypulse.robot.subsystems.arm.Arm;
+import com.stuypulse.stuylib.input.Gamepad;
+
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+/**
+ * automatically determines how to align depending on the state of the arm
+ * 
+ * <p> buzzes controller if arm is not in one of the scoring/ferrying positions
+ */
+public class SwerveDriveAutoAlignment extends InstantCommand{
+
+    private final Gamepad driver;
+
+    public SwerveDriveAutoAlignment(Gamepad driver) {
+        this.driver = driver;
+    }
+
+    @Override
+    public void initialize() {
+        switch (Arm.getInstance().getState()) {
+            case AMP:
+                CommandScheduler.getInstance().schedule(new SwerveDriveDriveAmpAligned(driver));
+                break;
+            case SPEAKER_HIGH:
+                CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerHigh(driver));
+                break;
+            case SPEAKER_LOW:
+                CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerLow(driver));
+                break;
+            case LOW_FERRY:
+                CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedLowFerry(driver));
+                break;
+            case LOB_FERRY:
+                CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedLobFerry(driver));
+                break;
+            default:
+                CommandScheduler.getInstance().schedule(new BuzzController(driver));
+                break;
+        }
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java
@@ -0,0 +1,25 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.input.Gamepad;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+
+public class SwerveDriveDriveAmpAligned extends SwerveDriveDriveAligned {
+
+    public SwerveDriveDriveAmpAligned(Gamepad driver) {
+        super(driver);
+    }
+
+
+    @Override
+    protected Rotation2d getTargetAngle() {
+        return Rotation2d.fromDegrees(Robot.isBlue() ? 90 : 270);
+    }
+
+    @Override
+    protected double getDistanceToTarget() {
+        return Settings.Swerve.Assist.AMP_WALL_SCORE_DISTANCE;
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -172,6 +172,8 @@ public interface Swerve {
         public interface Assist {
             SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); 
 
+            double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5);
+
             // angle PID
             SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0);
             SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0);