Pass Fuel from Neutral to Alliance Auto

simgui-ds.json

@@ -125,4 +125,4 @@
       "guid": "Keyboard1"
     }
   ]
-}
+}

src/main/deploy/choreo/autos.chor

@@ -57,8 +57,8 @@
   },
   "bumper":{
    "front":{
-    "exp":"16.25 in",
-    "val":0.41275
+    "exp":"23 in",
+    "val":0.5841999999999999
    },
    "side":{
     "exp":"16.5 in",

src/main/deploy/choreo/left_neutral_stopping.traj

@@ -96,7 +96,7 @@
    "tmax":0.75,
    "cof":1.0,
    "bumper":{
-    "front":0.41275,
+    "front":0.5841999999999999,
     "side":0.4191,
     "back":0.41275
    },

src/main/java/frc/robot/RobotContainer.java

@@ -129,6 +129,8 @@ private void configureAutonomousChooser() {
     autoChooser.addOption("Left Double Neutral Cycle", autonomous.leftDoubleNeutralCycle());
     autoChooser.addOption("Right Single Neutral Cycle", autonomous.rightSingleNeutralCycle());
     autoChooser.addOption("Right Double Neutral Cycle", autonomous.rightDoubleNeutralCycle());
+    autoChooser.addOption("Left Pass Fuel Cycle", autonomous.leftPassCycle());
+    autoChooser.addOption("Right Pass Fuel Cycle", autonomous.rightPassCycle());
     autoChooser.addOption("Preload", autonomous.preload());
 
     autoChooser.addOption("SysId Shooter Rollers", shooterRollers.sysId());
@@ -145,7 +147,6 @@ private void configureAutonomousChooser() {
         "SysId Front Drive", swerveDrive.driveSysId("Front Drive", true, true, false, false, 0));
     autoChooser.addOption(
         "SysId Back Drive", swerveDrive.driveSysId("Back Drive", false, false, true, true, 2));
-    autoChooser.addOption("Move Back From Hub and Shoot", autonomous.moveBackwardAndShoot());
     SmartDashboard.putData("Select Autonomous Routine", autoChooser);
   }
 

src/main/java/frc/robot/auto/Autonomous.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.RobotContainer;
 import frc.robot.auto.shoot.AutoShoot;
+import frc.robot.subsystems.hopper.sensors.HopperSensors;
 import java.util.function.Supplier;
 
 /** Contains autonomous command sequences that can be selected on the dashboard. */
@@ -21,6 +22,9 @@ public class Autonomous {
   public final AutoOutpost autoOutpost;
   public final AutoEdgeIntake autoEdgeIntake;
   public final CollectFuelFromHub collectFuelFromHub;
+  public final AutoShoot autoPassLeft;
+  public final AutoShoot autoPassRight;
+  public final HopperSensors hopperSensors;
 
   public Autonomous(RobotContainer robot) {
     this.robot = robot;
@@ -31,6 +35,9 @@ public Autonomous(RobotContainer robot) {
     this.autoOutpost = new AutoOutpost(robot, shootFuel);
     this.autoEdgeIntake = new AutoEdgeIntake(robot);
     this.collectFuelFromHub = new CollectFuelFromHub(robot);
+    this.autoPassLeft = new AutoShoot(robot, () -> AutoShoot.PASS_LEFT_TRANSLATION);
+    this.autoPassRight = new AutoShoot(robot, () -> AutoShoot.PASS_RIGHT_TRANSLATION);
+    this.hopperSensors = new HopperSensors();
   }
 
   public Command trenchCheck(
@@ -49,6 +56,7 @@ public Command trenchCheck(
                       && robotY > Inches.of(158.84).in(Meters)))
               || (Math.abs(robotRotation.getDegrees() - targetPose.getRotation().getDegrees())
                   > 15)) {
+
             return robot.getSwerveDrive().driveTo(targetPose).andThen(needsTrenchCheck.get());
           } else {
             return doesntNeedTrenchCheck.get();
@@ -58,13 +66,9 @@ public Command trenchCheck(
   }
 
   private static Pose2d LEFT_START_POSE =
-      new Pose2d(4.396968364715576, 7.652250289916992, new Rotation2d());
+      new Pose2d(4.436294078826904, 7.646793365478516, new Rotation2d());
 
   private Command singleNeutralCycle(boolean rightSide) {
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.0");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.1");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.2");
-
     return Commands.sequence(
         Commands.runOnce(
             () ->
@@ -78,65 +82,46 @@ private Command singleNeutralCycle(boolean rightSide) {
             .deadlineFor(
                 robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
         robot.getSwerveDrive().followPath("left_neutral.1", rightSide),
-        robot.getSwerveDrive().followPath("left_neutral.2", rightSide),
+        trenchCheck(
+            mirrorPose(
+                new Pose2d(6.273321628570557, 7.426932334899902, Rotation2d.kZero), rightSide),
+            () -> robot.getSwerveDrive().followPath("left_neutral_stopping.2", rightSide),
+            () -> robot.getSwerveDrive().followPath("left_neutral.2", rightSide)),
         shootFuel.shoot());
   }
 
   private Command doubleNeutralCycle(boolean rightSide) {
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.0");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.1");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.2");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.3");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.4");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.5");
-    robot.getSwerveDrive().loadChoreoPath("left_neutral.6");
-
-    return Commands.sequence(
-            Commands.runOnce(
-                () ->
-                    robot
-                        .getSwerveDrive()
-                        .getLocalization()
-                        .resetPosition(mirrorPose(LEFT_START_POSE, rightSide))),
-            robot
-                .getSwerveDrive()
-                .followPath("left_neutral.0", rightSide)
-                .deadlineFor(
-                    robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
-            robot
-                .getSwerveDrive()
-                .followPath("left_neutral.1", rightSide)
-                .deadlineFor(
-                    robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
-            robot.getSwerveDrive().followPath("left_neutral.2", rightSide),
-            shootFuel.shootAllFuelStationary().withTimeout(5),
-            robot
-                .getSwerveDrive()
-                .followPath("left_neutral.3", rightSide)
-                .deadlineFor(
-                    robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
-            robot
-                .getSwerveDrive()
-                .followPath("left_neutral.4", rightSide)
-                .deadlineFor(
-                    robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
-            robot
-                .getSwerveDrive()
-                .followPath("left_neutral.5", rightSide)
-                .deadlineFor(
-                    robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
-            robot.getSwerveDrive().followPath("left_neutral.6", rightSide),
-            shootFuel.shoot())
-        .withTimeout(20);
-  }
-
-  // Auto starting at the Mid-Hub position, moving backwards and then shooting.
-  public Command moveBackwardAndShoot() {
-    AutoShoot autoShoot = new AutoShoot(robot);
     return Commands.sequence(
-        robot.getSwerveDrive().driveTo(FieldPositions.HUB_FURTHER_FRONT),
-        Commands.parallel(
-            shootFuel.shoot().onlyWhile(autoShoot.isReadyToShoot()).repeatedly(), autoShoot));
+        Commands.runOnce(
+            () ->
+                robot
+                    .getSwerveDrive()
+                    .getLocalization()
+                    .resetPosition(mirrorPose(LEFT_START_POSE, rightSide))),
+        robot
+            .getSwerveDrive()
+            .followPath("left_neutral.0", rightSide)
+            .deadlineFor(
+                robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
+        robot.getSwerveDrive().followPath("left_neutral.1", rightSide),
+        trenchCheck(
+            mirrorPose(
+                new Pose2d(6.273321628570557, 7.426932334899902, Rotation2d.kZero), rightSide),
+            () -> robot.getSwerveDrive().followPath("left_neutral_stopping.2", rightSide),
+            () -> robot.getSwerveDrive().followPath("left_neutral.2", rightSide)),
+        shootFuel.shootAllFuelStationary(),
+        trenchCheck(
+                mirrorPose(new Pose2d(3.408, 7.5, Rotation2d.kZero), rightSide),
+                () -> robot.getSwerveDrive().followPath("left_neutral_stopping.3", rightSide),
+                () -> robot.getSwerveDrive().followPath("left_neutral.3", rightSide))
+            .deadlineFor(
+                robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
+        trenchCheck(
+            mirrorPose(
+                new Pose2d(5.575682163238525, 7.432374954223633, Rotation2d.k180deg), rightSide),
+            () -> robot.getSwerveDrive().followPath("left_neutral_stopping.4", rightSide),
+            () -> robot.getSwerveDrive().followPath("left_neutral.4", rightSide)),
+        shootFuel.shoot());
   }
 
   public Command leftSingleNeutralCycle() {
@@ -156,7 +141,58 @@ public Command rightDoubleNeutralCycle() {
   }
 
   public Command preload() {
-    return shootFuel.shoot();
+    return shootFuel
+        .shoot()
+        .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake());
+  }
+
+  public Command passCycle(boolean rightSide) {
+    return Commands.sequence(
+        Commands.runOnce(
+            () ->
+                robot
+                    .getSwerveDrive()
+                    .getLocalization()
+                    .resetPosition((mirrorPose(LEFT_START_POSE, rightSide)))),
+        // shootFuel.shoot(),
+        robot.getSwerveDrive().followPath("pass_cycle.0", rightSide),
+        robot
+            .getSwerveDrive()
+            .followPath("pass_cycle.1", rightSide)
+            .deadlineFor(
+                robot.getIntakeExtension().extend(),
+                robot.getIntakeRollers().intakeFast(),
+                rightSide ? autoPassLeft : autoPassRight,
+                robot.getHopper().feed()),
+        Commands.either(
+                Commands.sequence(robot.getSwerveDrive().followPath("pass_cycle_full.2", rightSide),
+                trenchCheck(mirrorPose(new Pose2d(6.009798049926758, 7.468980312347412, Rotation2d.kZero), rightSide), 
+                () -> robot.getSwerveDrive().followPath("pass_cycle_full_stopping.3", rightSide), 
+                () -> robot.getSwerveDrive().followPath("pass_cycle_full.3", rightSide))),
+                Commands.sequence(robot.getSwerveDrive().followPath("pass_cycle.2", rightSide), 
+                trenchCheck(mirrorPose(new Pose2d(6.009798049926758, 7.468980312347412, Rotation2d.k180deg), rightSide), 
+                () -> robot.getSwerveDrive().followPath("pass_cycle_stopping.3", rightSide), 
+                () -> robot.getSwerveDrive().followPath("pass_cycle.3", rightSide))),
+                () -> hopperSensors.isHopperFull())
+            .deadlineFor(
+                robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
+        robot.getSwerveDrive().followPath("pass_cycle.4", rightSide),
+        robot
+            .getSwerveDrive()
+            .followPath("pass_cycle.5", rightSide)
+            .deadlineFor(
+                robot.getIntakeExtension().extend(),
+                robot.getIntakeRollers().intake(),
+                shootFuel.shoot()),
+        shootFuel.shoot());
+  }
+
+  public Command leftPassCycle() {
+    return passCycle(false);
+  }
+
+  public Command rightPassCycle() {
+    return passCycle(true);
   }
 
   private static Pose2d mirrorPose(Pose2d pose, boolean mirrored) {

src/main/java/frc/robot/auto/FieldPositions.java

@@ -69,8 +69,7 @@ public static class Bump {
           Inches.of(182.11).in(Meters), Inches.of(158.84).in(Meters));
   public static final Translation2d HUB_BACK =
       new Translation2d(Inches.of(230.61).in(Meters), Inches.of(158.84).in(Meters));
-  public static final Translation2d HUB_FURTHER_FRONT =
-      new Translation2d(Inches.of(82.11).in(Meters), Inches.of(158.84).in(Meters));
+
   public static final Translation2d LEFT_END_OF_NEUTRAL_ZONE =
       new Translation2d(
           Inches.of(439.11).in(Meters), Inches.of(292.76).in(Meters)); // needs to be tuned

src/main/java/frc/robot/auto/ShootFuel.java

@@ -39,26 +39,10 @@ public Command shootAllFuelStationary() {
             autoShoot,
             Commands.waitUntil(
                     () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation())
-                .andThen(robot.getHopper().feed().repeatedly()),
-            Commands.sequence(
-                pulseIntake(),
-                Commands.sequence(
-                        robot.getIntakeExtension().retract(), robot.getIntakeExtension().extend())
-                    .repeatedly()),
-            robot.getIntakeRollers().intake())
+                .andThen(robot.getHopper().feed().repeatedly()))
         .until(() -> robot.getHopper().isEmpty());
   }
 
-  private Command pulseIntake() {
-    return Commands.sequence(
-        Commands.waitSeconds(2),
-        Commands.sequence(
-                robot.getIntakeExtension().retractSlow().withTimeout(0.5),
-                Commands.waitSeconds(0.25),
-                robot.getIntakeExtension().extendSlow().withTimeout(0.5))
-            .repeatedly());
-  }
-
   /**
    * Aligns the shooter then feeds fuel. This command does not end on its own.
    *
@@ -71,48 +55,6 @@ public Command shoot() {
         autoShoot,
         Commands.waitUntil(
                 () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation())
-            .andThen(robot.getHopper().feed().repeatedly()),
-        pulseIntake());
-  }
-
-  public Command shootOnTheMove() {
-    AutoShoot autoShoot = new AutoShoot(robot);
-
-    return Commands.parallel(
-        autoShoot,
-        Commands.waitUntil(
-                () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation())
-            .andThen(
-                robot
-                    .getHopper()
-                    .feed()
-                    .onlyWhile(
-                        () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation())
-                    .repeatedly()));
-  }
-
-  public Command shootAllFuelOnTheMove() {
-    AutoShoot autoShoot = new AutoShoot(robot);
-
-    return Commands.parallel(
-            autoShoot,
-            Commands.waitUntil(
-                    () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation())
-                .andThen(
-                    robot
-                        .getHopper()
-                        .feed()
-                        .onlyWhile(
-                            () ->
-                                autoShoot.isReadyToShoot().getAsBoolean()
-                                    || RobotBase.isSimulation())
-                        .repeatedly()),
-            Commands.sequence(
-                pulseIntake(),
-                Commands.sequence(
-                        robot.getIntakeExtension().retract(), robot.getIntakeExtension().extend())
-                    .repeatedly()),
-            robot.getIntakeRollers().intake())
-        .until(() -> robot.getHopper().isEmpty());
+            .andThen(robot.getHopper().feed().repeatedly()));
   }
 }

src/main/java/frc/robot/auto/shoot/AutoShoot.java

@@ -38,11 +38,12 @@ public class AutoShoot extends Command {
   public static Translation2d HUB_TRANSLATION =
       new Translation2d(4.62403825, 4.03463125); // Measured with CAD
 
-  public static Translation2d PASS_LEFT_TRANSLATION = new Translation2d(2, 2);
+  public static Translation2d PASS_RIGHT_TRANSLATION = new Translation2d(2, 2);
 
-  public static Translation2d PASS_RIGHT_TRANSLATION =
+  public static Translation2d PASS_LEFT_TRANSLATION =
       new Translation2d(
-          PASS_LEFT_TRANSLATION.getX(), HUB_TRANSLATION.getY() * 2 - PASS_LEFT_TRANSLATION.getY());
+          PASS_RIGHT_TRANSLATION.getX(),
+          HUB_TRANSLATION.getY() * 2 - PASS_RIGHT_TRANSLATION.getY());
 
   /**
    * The swerve drive subsystem, used to get the shooter's pose and velocity for calculating

src/main/java/frc/robot/controls/TeleopControls.java

@@ -351,8 +351,8 @@ public void configureBindings() {
             robot.getPassFunctions(),
             () ->
                 robot.getSwerveDrive().getPosition2d().getY() > AutoShoot.HUB_TRANSLATION.getY()
-                    ? AutoShoot.PASS_RIGHT_TRANSLATION
-                    : AutoShoot.PASS_LEFT_TRANSLATION,
+                    ? AutoShoot.PASS_LEFT_TRANSLATION
+                    : AutoShoot.PASS_RIGHT_TRANSLATION,
             () -> tunableHoodAngle == 0 ? null : Degrees.of(tunableHoodAngle),
             () -> tunableRollerVelocity == 0 ? null : RotationsPerSecond.of(tunableRollerVelocity));
 

src/main/java/frc/robot/subsystems/hopper/HopperConstants.java

@@ -77,7 +77,7 @@ public class HopperConstants {
   public static final Distance KICKER_SENSOR_FULL_THRESHOLD = Inches.of(5.0);
 
   // Threshold distances for determining if the Kicker is empty
-  public static final Distance KICKER_SENSOR_EMPTY_THRESHOLD = Inches.of(13.0);
+  public static final Distance KICKER_SENSOR_EMPTY_THRESHOLD = Inches.of(17.0);
 
   // Threshold distances for determining if the Hopper is full
   public static final Distance UPPER_HOPPER_SENSOR_FULL_THRESHOLD =