Ferry command (#87)

* added auto ferry command Co-authored-by: Lapcas <[email protected]> Co-authored-by: alex <[email protected]> Co-authored-by: Rahel Arka <[email protected]> Co-authored-by: jopy-man <[email protected]> Co-authored-by: rzheng287 <[email protected]> Co-authored-by: hahafoot <[email protected]> Co-authored-by: simon <[email protected]> Co-authored-by: urnotkool <[email protected]> * add override end method * Fix command, bind to top button --------- Co-authored-by: Naowal Rahman <[email protected]> Co-authored-by: Lapcas <[email protected]> Co-authored-by: alex <[email protected]> Co-authored-by: Rahel Arka <[email protected]> Co-authored-by: jopy-man <[email protected]> Co-authored-by: rzheng287 <[email protected]> Co-authored-by: hahafoot <[email protected]> Co-authored-by: simon <[email protected]> Co-authored-by: urnotkool <[email protected]>
StuyPulse · Mar 23, 2024 · b301ed9 · b301ed9
1 parent 26cff58
commit b301ed9
Show file tree

Hide file tree

Showing 6 changed files with 113 additions and 14 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -187,20 +187,23 @@ private void configureDriverBindings() {
             .onFalse(new AmperStop());
 
         // automatic swerve drive
-        driver.getTopButton()
-            // on command start
-            .onTrue(new BuzzController(driver, Assist.BUZZ_INTENSITY)
-                .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))
+        // driver.getTopButton()
+        //     // on command start
+        //     .onTrue(new BuzzController(driver, Assist.BUZZ_INTENSITY)
+        //         .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))
 
-            .onTrue(new SwerveDriveAutomatic(driver)
-                // after command end
-                .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
-                    .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))
+        //     .onTrue(new SwerveDriveAutomatic(driver)
+        //         // after command end
+        //         .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
+        //             .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))
 
-                .andThen(new WaitCommand(Driver.Drive.BUZZ_DURATION))
-
-                .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
-                    .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))));
+        //         .andThen(new WaitCommand(Driver.Drive.BUZZ_DURATION))
+
+        //         .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
+        //             .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))));
+
+        driver.getTopButton()
+            .whileTrue(new SwerveDriveAutoFerry(driver));
 
         // climb
         driver.getRightButton()

diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java
@@ -18,7 +18,7 @@ public class ConveyorShoot extends InstantCommand {
 
     public static Command untilDone() {
         return new ConveyorShoot()
-            .andThen(new WaitUntilCommand(() -> Shooter.getInstance().noteShot()));
+            .andThen(new WaitUntilCommand(Shooter.getInstance()::noteShot));
     }
 
     private final Conveyor conveyor;

diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java
@@ -16,7 +16,7 @@ public ConveyorShootRoutine() {
 
     public ConveyorShootRoutine(double delay) {
         addCommands(
-            new InstantCommand(() -> SwerveDrive.getInstance().stop(), SwerveDrive.getInstance()),
+            new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()),
             new ConveyorShoot(),
             new WaitCommand(delay),
             new ConveyorStop(),

diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java
@@ -0,0 +1,73 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.subsystems.conveyor.Conveyor;
+import com.stuypulse.robot.subsystems.intake.Intake;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.input.Gamepad;
+
+import com.stuypulse.robot.constants.Settings;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+
+public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned {
+
+    private final Odometry odometry;
+    private final Shooter shooter;
+    private final Conveyor conveyor;
+    private final Intake intake;
+
+    public SwerveDriveAutoFerry(Gamepad driver) {
+        super(driver);
+
+        shooter = Shooter.getInstance();
+        odometry = Odometry.getInstance();
+        conveyor = Conveyor.getInstance();
+        intake = Intake.getInstance();
+
+        addRequirements(shooter, odometry);
+    }
+
+    // returns pose of close amp corner
+    private Translation2d getTargetPose() {
+        return Robot.isBlue()
+            ? new Translation2d(0, Field.WIDTH)
+            : new Translation2d(0, 0);
+    }
+
+    @Override
+    public Rotation2d getTargetAngle() {
+        return getTargetPose().minus(odometry.getPose().getTranslation()).getAngle();
+    }
+
+    @Override
+    public double getDistanceToTarget() {
+        return getTargetPose().getDistance(odometry.getPose().getTranslation());
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+
+        if (odometry.getPose().getX() < Field.FERRY_SHOT_THRESHOLD_X) {
+            intake.acquire();
+            conveyor.toShooter();
+            shooter.setTargetSpeeds(Settings.Shooter.FERRY);
+        } else {
+            intake.stop();
+            conveyor.stop();
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.stop();
+        conveyor.stop();
+        shooter.setTargetSpeeds(Settings.Shooter.PODIUM_SHOT);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java
@@ -9,6 +9,7 @@
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.util.vision.AprilTag;
+import com.stuypulse.stuylib.network.SmartNumber;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -185,6 +186,18 @@ public static AprilTag getAllianceAmpTag() {
         return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP).tag;
     }
 
+    /*** SOURCE ***/
+
+    public static Pose2d getAllianceSourcePose() {
+        return (Robot.isBlue() ? NamedTags.BLUE_SOURCE_RIGHT : NamedTags.RED_SOURCE_RIGHT)
+            .getLocation().toPose2d();
+    }
+
+    public static Pose2d getOpposingSourcePose() {
+        return (Robot.isBlue() ? NamedTags.RED_SOURCE_RIGHT : NamedTags.BLUE_SOURCE_RIGHT)
+            .getLocation().toPose2d();
+    }
+
     /*** TRAP ***/
 
     public static Pose2d[] getAllianceTrapPoses() {
@@ -257,6 +270,10 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria
 
     double NOTE_BOUNDARY = LENGTH / 2 + Units.inchesToMeters(Settings.LENGTH / 2);
 
+    /*** FERRYING ***/
+
+    double FERRY_SHOT_THRESHOLD_X = 9.0;
+
     /**** EMPTY FIELD POSES ****/
 
     Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d());

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -319,6 +319,12 @@ public interface Shooter {
 
         ShooterSpeeds REVERSE = new ShooterSpeeds(-3000, -3000);
 
+        // xxx: determine
+        ShooterSpeeds FERRY = new ShooterSpeeds(
+            new SmartNumber("Shooter/Ferry Shooter RPM", 5500),
+            500,
+            new SmartNumber("Shooter/Ferry Feeder RPM", 3000));
+
         double AT_RPM_EPSILON = 125;
 
         SmartNumber RPM_CHANGE_RC = new SmartNumber("Shooter/RPM Change RC", 0.2);