whoops i should have done this earlier

src/main/deploy/pathplanner/paths/Shoot2 To Climb.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.5481222222222222,
-        "y": 3.752822222222222
+        "x": 0.7419,
+        "y": 2.664422222222222
       },
       "prevControl": {
-        "x": 2.0147248545881693,
-        "y": 2.880077770320452
+        "x": 1.2085026323659471,
+        "y": 1.7916777703204518
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,10 +42,10 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -0.939190945735607
+    "rotation": 180.0
   },
   "reversed": false,
-  "folder": "Neutral Zone Climb",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
     "rotation": 59.058463533677475

src/main/deploy/pathplanner/paths/ShootToClimb.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.5380444444444445,
-        "y": 3.7125111111111115
+        "x": 1.3667222222222224,
+        "y": 4.871455555555555
       },
       "prevControl": {
-        "x": 1.920864779920239,
-        "y": 3.7581512560925034
+        "x": 1.7495425576980168,
+        "y": 4.917095700536947
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -288,8 +288,8 @@ public RobotContainer() {
         autoChooser_ = new LoggedDashboardChooser<>("Auto Choices");
 
         autoChooser_.onChange(auto -> {
-            System.err.println("Auto \"" + auto.getName() + "\" selected!");
-            // This should be used to set up robot position setting, initialization, etc.
+            System.out.println("Auto \"" + auto.getName() + "\" selected!");
+            // Anything you may want to do when the auto is selected.
         });
 
         // Test Bindings
@@ -326,6 +326,9 @@ private void configureBindings() {
 
         // When the shooter isnt shooting, get it ready to shoot.
         shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose));
+
+        //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it.
+        gamepad_.a().whileTrue(intake_.ejectSequence());
     }
 
     private void configureDriveBindings() {

src/main/java/frc/robot/commands/drive/DriveCommands.java

@@ -62,6 +62,7 @@
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.util.MapleSimUtil;
 
 public class DriveCommands {
   private static final double kStoppedVelocity = 0.15 ;
@@ -593,10 +594,15 @@ public static Command initialFollowPathCommand(Drive drive, String pathName, boo
         return Commands.none();
       }
 
+      var startingPose = initPosePath.getStartingHolonomicPose().orElseThrow();
+
       return Commands.sequence(
           setPoseCommand(
               drive,
-              initPosePath.getStartingHolonomicPose().orElseThrow(), false),
+              startingPose,
+              false
+          ),
+          Commands.runOnce(() -> MapleSimUtil.placeRobotOnField(startingPose)),
           AutoBuilder.followPath(path.get()));
     }
 

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -52,6 +52,7 @@ public final class IntakeConstants {
 
     public static final AngularVelocity rollerMaxVelocity= DegreesPerSecond.of(360); //Temporary speed to be changed as needed 
     public static final Voltage rollerCollectVoltage= Volts.of(6); //Temporary voltage
+    public static final Voltage rollerEjectVoltage= Volts.of(-6); //Temporary voltage
 
     public static final Angle pivotTolerance = Degrees.of(5); //Tolerance to compare current angle to target
 

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -69,6 +69,10 @@ private void stopIntaking() {
         io.stopRoller();
     }
 
+    private void eject(){
+        io.setRollerVoltage(IntakeConstants.rollerEjectVoltage);
+    }
+
     /**
      * Stows the intake and hopper.
      */
@@ -118,6 +122,14 @@ public Command stowCmd() {
             .withTimeout(2)).withName("Stow Intake");
     }
 
+    /**
+     * Command that stops the rollers, mainly for in automodes
+     * @return
+     */
+    public Command stopIntake(){
+        return runOnce(() -> stopIntaking()).withName("Stop Rollers");
+    }
+
     /**
      * Command that deploys the intake and ends when it gets there.
      * @return
@@ -150,6 +162,16 @@ public Command intakeSequence() {
         ).finallyDo(interrupted -> waiting());
     }
 
+    public Command runEjectCmd() {
+        return startEnd(this::eject, this::stopIntaking);
+    }
+
+    public Command ejectSequence() {
+        return runEjectCmd().beforeStarting(
+            deployCmd().unless(this::isIntakeDeployed)
+        ).finallyDo(interrupted -> waiting());
+    }
+
     ////////////////////////////
     /// Sys ID Routine creation/
     /// ////////////////////////

src/main/java/frc/robot/util/MapleSimUtil.java

@@ -137,6 +137,10 @@ public static Pose2d getPosition() {
         return drivebaseSimulation.getSimulatedDriveTrainPose();
     }
 
+    public static void placeRobotOnField(Pose2d pose) {
+        drivebaseSimulation.setSimulationWorldPose(pose);
+    }
+
     public static ChassisSpeeds getFieldChassisSpeeds() {
         return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative();
     }