feat: full path interupt, spit shots, drive with correction addition

src/main/java/org/team1540/robot2024/Constants.java

@@ -109,9 +109,10 @@ public static class Indexer {
     public static class Vision {
         public static final String FRONT_CAMERA_NAME = "limelight-front";
         public static final String REAR_CAMERA_NAME = "limelight-rear";
-
+        public static final String VISION_CAMERA_NAME = "limelight-vision";
         public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0));
         public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(Math.PI, Math.toRadians(10), Math.PI+Math.toRadians(1.55)));
+        public static final Pose3d VISION_CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
 
         public static final boolean TAKE_SNAPSHOTS = true;
         public static final double SNAPSHOT_PERIOD_SECS = 1;

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -33,6 +33,7 @@
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.auto.AutoCommand;
 import org.team1540.robot2024.util.auto.AutoManager;
+import org.team1540.robot2024.util.vision.LimelightHelpers;
 
 import java.util.function.BooleanSupplier;
 
@@ -166,14 +167,9 @@ private void configureButtonBindings() {
         if (isTuningMode()) {
 //            driver.leftTrigger().whileTrue(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(new LedPatternWave("#00ff00"), Leds.PatternLevel.DRIVER_LOCK)));
             driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose));
-//            driver.leftTrigger().whileTrue(new DriveWithCorrectionCommand(drivetrain, driver, ()->drivetrain.getPose()
-//                    .minus(Constants.Targeting.getSpeakerPose()).getTranslation().getAngle()
-//                    .rotateBy(DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0))
-//                    .minus(drivetrain.getRotation()).getDegrees()
-//                    )
-//            );
 //            drivetrain.getRotation();
-            driver.leftTrigger().whileTrue(new DriveWithCorrectionCommand(drivetrain, driver, ()-> 10.0));
+//            driver.leftTrigger().whileTrue(new DriveWithCorrectionCommand(drivetrain, driver, ()-> LimelightHelpers.getTX(Constants.Vision.VISION_CAMERA_NAME)));
+            driver.leftTrigger().whileTrue(new SpitShoot(shooter, indexer));
         }
 
         driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true));

src/main/java/org/team1540/robot2024/commands/autos/ATestAuto.java

@@ -30,27 +30,28 @@ public ATestAuto(Drivetrain drivetrain, Shooter shooter, Indexer indexer) {
 //                getPath(0).getCommand(drivetrain),
 
                 getPath(0).withInterrupt(
-                        Commands.sequence(
+                        ()->Commands.sequence(
                                 getPath(0).getCommand(drivetrain),
                                 Commands.waitSeconds(1),
                                 getPath(1).withInterrupt(
+                                        ()->
                                         Commands.sequence(
                                                 getPath(1).getCommand(drivetrain),
                                                 Commands.waitSeconds(1),
                                                 getPath(2).getCommand(drivetrain)
                                         ),
-                                        getPath(4).getCommand(drivetrain, false),
-                                        new Triplet<>(0, ()->true, Commands.none()),
-                                        new Triplet<>(1, ()->false, Commands.none())
+                                        ()->getPath(4).getCommand(drivetrain, false),
+                                        new Triplet<>(0, ()->true, Commands::none),
+                                        new Triplet<>(1, ()->false, Commands::none)
                                 )
                         ),
-                        Commands.sequence(
+                        ()->Commands.sequence(
                                 getPath(3).getCommand(drivetrain, false),
                                 Commands.waitSeconds(1),
                                 getPath(2).getCommand(drivetrain)
                         ),
-                        new Triplet<>(0, ()->true, Commands.none()),
-                        new Triplet<>(1, ()->false, Commands.none())
+                        new Triplet<>(0, ()->true, Commands::none),
+                        new Triplet<>(1, ()->false, Commands::none)
                 ),
                 Commands.print("HALLELUJAH")
 

src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithCorrectionCommand.java

@@ -1,7 +1,9 @@
 package org.team1540.robot2024.commands.drivetrain;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.littletonrobotics.junction.Logger;
@@ -11,29 +13,54 @@
 
 import java.util.function.Supplier;
 
+import static org.team1540.robot2024.Constants.Targeting.*;
+
 public class DriveWithCorrectionCommand extends Command {
     private final Drivetrain drivetrain;
     private final CommandXboxController controller;
 
     private final Supplier<Double> angleDegrees;
+    private final Supplier<Pose2d> target;
     private static final double deadzone = 0.03;
 
-    private static final double yawScale = 0.5;
+    private boolean isFlipped;
 
-    public DriveWithCorrectionCommand(Drivetrain drivetrain, CommandXboxController controller, Supplier<Double> angleDegrees) {
+    private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD);
+
+
+    public DriveWithCorrectionCommand(Drivetrain drivetrain, CommandXboxController controller, Supplier<Double> angleDegrees, Supplier<Pose2d> target) {
 
         this.drivetrain = drivetrain;
         this.controller = controller;
         this.angleDegrees = angleDegrees;
+        this.target = target;
+        rotController.enableContinuousInput(-Math.PI, Math.PI);
         addRequirements(drivetrain);
     }
 
+    public DriveWithCorrectionCommand(Drivetrain drivetrain, CommandXboxController controller, Supplier<Double> angleDegrees) {
+        this(drivetrain, controller, angleDegrees, null);
+    }
+
     @Override
     public void initialize() {
+        rotController.reset();
+        isFlipped = Constants.Targeting.getFlipped();
     }
 
     @Override
     public void execute() {
+        Rotation2d targetRot = null;
+        if(target != null){
+             targetRot =
+                    drivetrain.getPose()
+                            .minus(target.get()).getTranslation().getAngle()
+                            .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0));
+            drivetrain.setTargetPose(new Pose2d(drivetrain.getPose().getTranslation(), targetRot));
+            Logger.recordOutput("Targeting/rotError", targetRot.minus(drivetrain.getRotation()));
+            Logger.recordOutput("Targeting/target", target.get());
+        }
+
         double xPercent   = -controller.getLeftY() * (Constants.IS_COMPETITION_ROBOT ? 1 : -1);
 
         Rotation2d linearDirection = drivetrain.getRotation().rotateBy(Rotation2d.fromDegrees(angleDegrees.get()));
@@ -43,7 +70,9 @@ public void execute() {
                 linearDirection
         ));
 
-        double rotPercent = JoystickUtils.smartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1);
+        double rotPercent = target == null
+                ? JoystickUtils.smartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1)
+                : rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians());;
         drivetrain.drivePercent((xPercent)*(linearDirection.getCos()>0?1:-1), linearDirection, rotPercent, true);
     }
 

src/main/java/org/team1540/robot2024/commands/shooter/SpitShoot.java

@@ -0,0 +1,26 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.commands.indexer.IntakeAndFeed;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+import org.team1540.robot2024.util.shooter.ShooterSetpoint;
+import org.team1540.robot2024.util.vision.AprilTagsCrescendo;
+
+import java.util.function.Supplier;
+
+public class SpitShoot extends ParallelCommandGroup {
+    public SpitShoot(Shooter shooter, Indexer indexer, double leftSetpoint, double rightSetpoint, double indexerPercent, double feederPercent) {
+        addCommands(
+                new IntakeAndFeed(indexer, ()->indexerPercent, ()->feederPercent),
+                new PrepareShooterCommand(shooter, ()-> new ShooterSetpoint(0, leftSetpoint, rightSetpoint))
+        );
+    }
+
+    public SpitShoot(Shooter shooter, Indexer indexer){
+        this(shooter, indexer, Constants.Shooter.Flywheels.LEFT_RPM, Constants.Shooter.Flywheels.RIGHT_RPM, 1, 1);
+    }
+}

src/main/java/org/team1540/robot2024/commands/shooter/SpitShootWithTargeting.java

@@ -0,0 +1,18 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand;
+import org.team1540.robot2024.subsystems.drive.Drivetrain;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+public class SpitShootWithTargeting extends ParallelCommandGroup {
+    public SpitShootWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter, Indexer indexer) {
+        addCommands(
+                new DriveWithTargetingCommand(drivetrain,controller, Constants.Targeting::getSpeakerPose),
+                new SpitShoot(shooter, indexer)
+        );
+    }
+}

src/main/java/org/team1540/robot2024/util/auto/PathHelper.java

@@ -89,18 +89,19 @@ public Command getCommand(Drivetrain drivetrain, boolean shouldRealign) {
         return (isResetting ? resetCommand.andThen(command) : command);
     }
 
-    public Command withInterrupt(Drivetrain drivetrain, boolean shouldRealign, Triplet<Integer, BooleanSupplier, Command>... terms){
-        return withInterrupt(getCommand(drivetrain, shouldRealign), Commands.none(), terms);
-    }
+//    public Command withInterrupt(Drivetrain drivetrain, boolean shouldRealign, Triplet<Integer, BooleanSupplier, Command>... terms){
+//        return withInterrupt(getCommand(drivetrain, shouldRealign), Commands.none(), terms);
+//    }
 
-    public Command withInterrupt(Command cmd, Command afterInterrupt, Triplet<Integer, BooleanSupplier, Command>... terms){
+    public Command withInterrupt(Supplier<Command> cmd, Supplier<Command> afterInterrupt, Triplet<Integer, BooleanSupplier, Supplier<Command>>... terms){
         //FIXME MAJOR ISSUE MAYBE Command always causes crash on second auto run but theoretically it should be fine because we only run once
         Arrays.sort(terms, (o1, o2) -> -1* Double.compare(eventMarkers.get(o1.getFirst()).getWaypointRelativePos(), eventMarkers.get(o2.getFirst()).getWaypointRelativePos()));
+        Command tempCommand = cmd.get();
         for(int i = 0; i < terms.length; i += 1){
             Triplet term = terms[i];
             final boolean[] cancel = {false};
-            cmd = Commands.race(
-                    cmd,
+            tempCommand = Commands.race(
+                    tempCommand,
                     Commands.sequence(
                             Commands.waitSeconds(eventMarkers.get((int)term.getFirst()).getWaypointRelativePos()),
                             new ConditionalCommand(
@@ -113,8 +114,10 @@ public Command withInterrupt(Command cmd, Command afterInterrupt, Triplet<Intege
 //                    Commands.runOnce(()->System.out.println(cancel[0].getAsBoolean())),
                     Commands.sequence(
 //                            Commands.runOnce(()->System.out.println(cancel[0].getAsBoolean())),
-                            Commands.defer(()-> (Command)term.getThird(), ((Command)term.getThird()).getRequirements()),
-                            Commands.defer(()->afterInterrupt, afterInterrupt.getRequirements())
+                            ((Supplier<Command>) term.getThird()).get(),
+//                            Commands.defer(()-> (Command)term.getThird(), ((Command)term.getThird()).getRequirements()),
+                            afterInterrupt.get()
+//                            Commands.defer(()->afterInterrupt.get(), afterInterrupt.get().getRequirements())
 //                            Commands.none()
 //                            ,after
 //                            ,afterInterrupt
@@ -127,7 +130,7 @@ public Command withInterrupt(Command cmd, Command afterInterrupt, Triplet<Intege
 //                    )
             );
         }
-        return cmd;
+        return tempCommand;
     }
 
     public Command resetToInitialPose(Drivetrain drivetrain){