scoring automation 4 pilot

untested code

src/main/java/frc/robot/intakeLauncher/commands/IntakeCommands.java

@@ -66,7 +66,7 @@ public static Command autoEject() {
                         Intake.config.frontEjectSpeed,
                         Intake.config.launcherEjectSpeed)
                 .alongWith(FourBarCommands.home().alongWith(ElevatorCommands.simpleSafeHome()))
-                .withName("Eject");
+                .withName("AutoEject");
     }
 
     public static Command floorEject() {

src/main/java/frc/robot/mechanisms/MechanismsCommands.java

@@ -1,10 +1,14 @@
 package frc.robot.mechanisms;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.elevator.Elevator;
 import frc.robot.elevator.commands.ElevatorCommands;
+import frc.robot.fourbar.FourBar;
 import frc.robot.fourbar.commands.FourBarCommands;
+import frc.robot.fourbar.commands.FourBarDelay;
 import frc.robot.intakeLauncher.commands.ConeIntake;
 import frc.robot.intakeLauncher.commands.CubeIntake;
+import frc.robot.intakeLauncher.commands.IntakeCommands;
 import frc.robot.swerve.commands.SwerveDrive;
 
 public class MechanismsCommands {
@@ -42,13 +46,63 @@ public static Command cubeIntake() {
                                                         () -> 0,
                                                         () -> 0,
                                                         () -> 1.0, // full velocity
-                                                        () -> false // Robot Relative
+                                                        () -> false // drive robot relative
                                                         )
                                                 .withTimeout(1)
                                                 .andThen(homeSystems().withTimeout(1))))
                 .withName("MechanismsStandingCone");
     }
 
+    public static Command cubeFloor() {
+        return ElevatorCommands.cubeFloorGoal()
+        .alongWith(FourBarCommands.cubeFloorGoal(), IntakeCommands.cubeFloorLaunch()).withTimeout(0.5).andThen(homeSystems())
+        .withName("MechanismsCubeFloor");
+    }
+
+    public static Command cubeMid() {
+        return IntakeCommands.intake()
+                .withTimeout(0.1)
+                .andThen(IntakeCommands.midCubeSpinUp().alongWith(ElevatorCommands.cubeMid()))
+                .withTimeout(1.5)
+                .withName("MechanismsCubeMid");
+    }
+
+    public static Command cubeTop() {
+        return IntakeCommands.intake()
+        .withTimeout(0.1)
+        .andThen(IntakeCommands.topCubeSpinUp().alongWith(ElevatorCommands.cubeTop())).withTimeout(1.5)
+        .withName("MechanismsCubeTop");
+    }
+
+    public static Command coneMid() {
+        return IntakeCommands.slowIntake()
+        .alongWith(ElevatorCommands.coneMid(), FourBarCommands.coneMid()).withTimeout(1)
+        .withName("MechanismsConeMid");
+    }
+
+    public static Command coneTop() {
+        return IntakeCommands.slowIntake()
+        .alongWith(
+                ElevatorCommands.coneTop(),
+                new FourBarDelay(
+                        FourBar.config.safePositionForElevator,
+                        FourBar.config.coneTop,
+                        Elevator.config.safePositionForFourBar)).withTimeout(1.5)
+        .withName("MechanismsConeTop");
+    }
+
+    public static Command launchAndRetract() {
+        return IntakeCommands.autoLaunch()
+                .withTimeout(0.2)
+                .andThen(homeSystems())
+                .withTimeout(0.5)
+                .withName("MechanismsLaunch&Retract");
+    }
+
+    public static Command ejectAndRetract() {
+        return IntakeCommands.autoEject().withTimeout(1.5).withName("MechanismsEject&Retract");
+    }
+
     /** Goes to 0 */
     public static Command homeSystems() {
         return FourBarCommands.home()

src/main/java/frc/robot/pilot/PilotGamepad.java

@@ -12,7 +12,6 @@
 import frc.robot.leds.commands.OneColorLEDCommand;
 import frc.robot.mechanisms.MechanismsCommands;
 import frc.robot.pilot.commands.PilotCommands;
-import frc.robot.swerve.commands.AlignToVisionTarget;
 import frc.robot.swerve.commands.DriveToVisionTarget;
 import frc.robot.trajectories.commands.DistanceDrive;
 import frc.robot.vision.VisionConfig;
@@ -77,28 +76,24 @@ public void setupTeleopButtons() {
         gamepad.bButton.whileTrue(IntakeCommands.launch());
 
         /* Dpad */
-        gamepad.Dpad.Up.and(noBumpers().or(rightBumperOnly())).whileTrue(IntakeCommands.launch());
-        gamepad.Dpad.Down.and(noBumpers().or(rightBumperOnly())).whileTrue(IntakeCommands.eject());
+        gamepad.Dpad.Up.and(noBumpers()).whileTrue(IntakeCommands.launch());
+        gamepad.Dpad.Down.and(noBumpers()).whileTrue(IntakeCommands.eject());
         gamepad.Dpad.Left.and(noBumpers()).whileTrue(new DistanceDrive(Units.inchesToMeters(5)));
         gamepad.Dpad.Right.and(noBumpers()).whileTrue(new DistanceDrive(Units.inchesToMeters(-5)));
 
-        /* Aligning */
-        rightBumperOnly()
-                .whileTrue(
-                        new DriveToVisionTarget(0, VisionConfig.cubeDetectorPipeline)
-                                .andThen(MechanismsCommands.cubeIntake()));
-        rightBumperOnly()
-                .and(rightTrigger)
-                .whileTrue(
-                        new DriveToVisionTarget(0, VisionConfig.coneDetectorPipeline)
-                                .andThen(MechanismsCommands.coneStandingIntake()));
+        /* Automation */
+        rightBumperOnly().and(rightTrigger).whileTrue(PilotCommands.automatedConeIntake());
         rightBumperOnly()
                 .and(leftTrigger)
                 .whileTrue(
-                        new AlignToVisionTarget(
-                                () -> Robot.pilotGamepad.getDriveFwdPositive(),
-                                0,
-                                VisionConfig.aprilTagPipeline));
+                        PilotCommands.automatedCubeIntake());
+        //cube floor isn't bound
+        gamepad.Dpad.Up.and(rightBumperOnly()).whileTrue(PilotCommands.automatedCubeTop());
+        gamepad.Dpad.Left.and(rightBumperOnly()).whileTrue(PilotCommands.automatedConeTop());
+        gamepad.Dpad.Down.and(rightBumperOnly()).whileTrue(PilotCommands.automatedCubeMid());
+        gamepad.Dpad.Right.and(rightBumperOnly()).whileTrue(PilotCommands.automatedConeMid());
+        // .andThen(MechanismsCommands.ejectAndRetract()));
+
 
         /* Reorient */
         gamepad.Dpad.Up.and(leftBumperOnly()).whileTrue(PilotCommands.reorient(0));

src/main/java/frc/robot/pilot/commands/PilotCommands.java

@@ -4,11 +4,14 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.Robot;
 import frc.robot.elevator.Elevator;
+import frc.robot.mechanisms.MechanismsCommands;
 import frc.robot.pose.commands.PoseCommands;
+import frc.robot.swerve.commands.DriveToVisionTarget;
 import frc.robot.swerve.commands.HeadingLock;
 import frc.robot.swerve.commands.LockSwerve;
 import frc.robot.swerve.commands.SwerveCommands;
 import frc.robot.swerve.commands.SwerveDrive;
+import frc.robot.vision.VisionConfig;
 import java.util.function.DoubleSupplier;
 
 /** Should contain commands used only by the pilot controller and to rumble pilot command */
@@ -54,6 +57,50 @@ public static Command pilotHeadingLock() {
                 .withName("SnakeDrive");
     }*/
 
+    public static Command automatedConeIntake() {
+        return new DriveToVisionTarget(0, VisionConfig.coneDetectorPipeline)
+                .andThen(MechanismsCommands.coneStandingIntake())
+                .withName("PilotAutomatedConeIntake");
+    }
+
+    public static Command automatedCubeIntake() {
+        return new DriveToVisionTarget(0, VisionConfig.cubeDetectorPipeline)
+                .andThen(MechanismsCommands.cubeIntake())
+                .withName("PilotAutomatedCubeIntake");
+    }
+
+    public static Command automatedCubeFloor() {
+        return new DriveToVisionTarget(0, VisionConfig.aprilTagPipeline)
+        .andThen(MechanismsCommands.cubeFloor())
+        .withName("PilotAutomatedCubeFloor");
+    }
+
+    public static Command automatedCubeMid() {
+        return new DriveToVisionTarget(0, VisionConfig.aprilTagPipeline)
+                                .alongWith(MechanismsCommands.cubeMid())
+                                .andThen(MechanismsCommands.launchAndRetract()).withName("PilotAutomatedCubeMid");
+    }
+
+    public static Command automatedCubeTop() {
+        return new DriveToVisionTarget(0, VisionConfig.aprilTagPipeline)
+                                .alongWith(MechanismsCommands.cubeTop())
+                                .andThen(MechanismsCommands.launchAndRetract()).withName("PilotAutomatedCubeTop");
+    }
+
+    public static Command automatedConeMid() {
+        return new DriveToVisionTarget(0, VisionConfig.reflectivePipeline)
+        .andThen(MechanismsCommands.coneMid())
+        .withName("PilotAutomatedConeMid");
+    }
+
+    public static Command automatedConeTop() {
+        return new DriveToVisionTarget(0, VisionConfig.reflectivePipeline)
+        .andThen(MechanismsCommands.coneTop())
+        .withName("PilotAutomatedConeMid");
+    }
+
+
+
     /**
      * Drive the robot and control orientation using the right stick
      *