Minor comment fixes

.vscode/settings.json

@@ -57,5 +57,6 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.dependency.enableDependencyCheckup": false
+  "java.dependency.enableDependencyCheckup": false,
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

src/main/deploy/pathplanner/autos/MOVE B____ left to center.auto

@@ -5,15 +5,22 @@
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "intake down"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "left start to center"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
           }
         },
         {
@@ -54,6 +61,12 @@
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/autos/left to center.auto

@@ -5,15 +5,22 @@
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "intake down"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "left start to center"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
           }
         },
         {
@@ -54,6 +61,12 @@
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path

@@ -69,7 +69,7 @@
       },
       "prevControl": {
         "x": 8.08578683847011,
-        "y": 0.4907704016028374
+        "y": 0.49077040160283736
       },
       "nextControl": {
         "x": 8.318287488908608,

src/main/deploy/pathplanner/paths/back from center.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.507563451776648,
-        "y": 5.7523147208121825
+        "x": 3.68848717948718,
+        "y": 5.639692307692307
       },
       "isLocked": false,
-      "linkedName": "over bump"
+      "linkedName": "after center grab"
     },
     {
       "anchor": {
-        "x": 3.002954314720812,
-        "y": 5.310324873096447
+        "x": 2.979166666666668,
+        "y": 5.2327051282051285
       },
       "prevControl": {
-        "x": 4.052680203045686,
-        "y": 5.9825177664974625
+        "x": 3.0721923076923088,
+        "y": 5.721089743589744
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,12 +30,8 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.10454545454545605,
-      "rotationDegrees": -115.0
-    },
-    {
-      "waypointRelativePos": 0.7931818181818296,
-      "rotationDegrees": -115.0
+      "waypointRelativePos": 0.11569148936170148,
+      "rotationDegrees": -34.71279786419313
     }
   ],
   "constraintZones": [],
@@ -43,7 +39,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 2.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -56,8 +52,8 @@
   "reversed": false,
   "folder": null,
   "idealStartingState": {
-    "velocity": 0,
-    "rotation": -129.95754893082906
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
   "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/left start to center.path

@@ -8,23 +8,23 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.9108730964467004,
-        "y": 6.498172588832488
+        "x": 3.0213705583756347,
+        "y": 6.461340101522843
       },
       "isLocked": false,
       "linkedName": "start left"
     },
     {
       "anchor": {
-        "x": 2.5885888324873094,
+        "x": 3.0766192893401025,
         "y": 5.936477157360406
       },
       "prevControl": {
-        "x": 2.3675939086294413,
+        "x": 2.8556243654822344,
         "y": 6.28638578680203
       },
       "nextControl": {
-        "x": 2.8493791599284664,
+        "x": 3.3374096167812595,
         "y": 5.523559138911907
       },
       "isLocked": false,
@@ -36,24 +36,24 @@
         "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 3.565840773084476,
-        "y": 5.768729726877464
+        "x": 3.5645859798769006,
+        "y": 5.732650999050524
       },
       "nextControl": {
-        "x": 5.102406091370558,
-        "y": 5.687857868020306
+        "x": 5.120822335025381,
+        "y": 5.706274111675127
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": {
-        "x": 5.903512690355329,
-        "y": 5.807563451776649
+        "x": 5.931137055837564,
+        "y": 5.65102538071066
       },
       "nextControl": null,
       "isLocked": false,
@@ -70,15 +70,15 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 2.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 1.0,
+    "velocity": 2.0,
     "rotation": -129.95754893082906
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/over bump to pile move.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.897989847715735,
-        "y": 6.406091370558377
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
       },
       "isLocked": false,
       "linkedName": "over bump"
@@ -48,16 +48,16 @@
     },
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": {
-        "x": 7.375431472081217,
-        "y": 5.183713197969544
+        "x": 7.758358974358975,
+        "y": 5.616435897435898
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "over bump"
+      "linkedName": "after center grab"
     }
   ],
   "rotationTargets": [
@@ -74,7 +74,7 @@
     {
       "name": "Constraints Zone",
       "minWaypointRelativePos": 1.1243680485338854,
-      "maxWaypointRelativePos": 2.0,
+      "maxWaypointRelativePos": 2.077102803738317,
       "constraints": {
         "maxVelocity": 1.0,
         "maxAcceleration": 1.5,
@@ -95,16 +95,16 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 2.0,
-    "rotation": -129.95754893082906
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
   "reversed": false,
   "folder": null,
@@ -112,5 +112,5 @@
     "velocity": 0,
     "rotation": -129.95754893082906
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/over bump to pile.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.897989847715735,
-        "y": 6.406091370558377
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
       },
       "isLocked": false,
       "linkedName": "over bump"
     },
     {
       "anchor": {
-        "x": 7.017695431472081,
-        "y": 7.216406091370559
+        "x": 7.220274111675127,
+        "y": 7.299279187817259
       },
       "prevControl": {
-        "x": 6.017695431472081,
-        "y": 7.216406091370559
+        "x": 6.220274111675127,
+        "y": 7.299279187817259
       },
       "nextControl": {
-        "x": 8.017695431472081,
-        "y": 7.216406091370559
+        "x": 8.220274111675131,
+        "y": 7.299279187817259
       },
       "isLocked": false,
       "linkedName": null
@@ -36,28 +36,28 @@
         "y": 4.831502538071066
       },
       "prevControl": {
-        "x": 7.941269035532995,
-        "y": 6.042370558375635
+        "x": 7.952417879560978,
+        "y": 6.041784973535379
       },
       "nextControl": {
-        "x": 7.825248730964466,
-        "y": 3.620634517766498
+        "x": 7.846426395939085,
+        "y": 4.186934010152284
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": {
-        "x": 7.375431472081217,
-        "y": 5.183713197969544
+        "x": 7.598549873246986,
+        "y": 5.589703440094898
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "over bump"
+      "linkedName": "after center grab"
     }
   ],
   "rotationTargets": [
@@ -73,7 +73,7 @@
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.1243680485338854,
+      "minWaypointRelativePos": 1.3403967538322836,
       "maxWaypointRelativePos": 2.0,
       "constraints": {
         "maxVelocity": 1.0,
@@ -95,22 +95,22 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
-    "rotation": -129.95754893082906
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
-    "velocity": 0,
+    "velocity": 2.0,
     "rotation": -129.95754893082906
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -148,7 +148,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
 
@@ -276,7 +277,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
         shooter.setDefaultCommand(shooter.stop());
@@ -285,6 +287,8 @@ public class RobotContainer {
 
         intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
 
+       // secondary.leftStick().whileTrue(intakePivot.manualSpeed(() -> secondary.getLeftY()));
+
         driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
         driver.y().whileTrue(drivetrain.zeroHeading());
         driver.x().whileTrue(drivetrain.setX());
@@ -293,7 +297,8 @@ public class RobotContainer {
             drivetrain.lockRotationToHub(
                 driver::getLeftY, 
                 driver::getLeftX, 
-                false
+                false,
+                true
             ) 
         );
 
@@ -301,11 +306,16 @@ public class RobotContainer {
         driver.rightBumper().whileTrue(intakeRoller.runIn());
 
         driver.rightTrigger().whileTrue(
-            spindexer.spinToShooter().alongWith(
-                intakeRoller.runIn(),
-                intakePivot.jimmy(.5)
+            spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
+                intakeRoller.runIn()/* ,
+                intakePivot.jimmy(.5)*/
             )
         );
+        driver.rightTrigger().whileTrue(
+            intakePivot.jimmy(.5)
+        );
+
+        secondary.leftBumper().onTrue(new InstantCommand(() -> {}, intakePivot));
 
         driver.rightTrigger().onFalse(
             intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
@@ -435,8 +445,21 @@ public class RobotContainer {
                 
             ).withTimeout(3).andThen(spindexer.instantaneousStop()));
 
-        NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter)
-            .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), spindexer.spinToShooter(), intakePivot.jimmy(0.5)));
+        NamedCommands.registerCommand("aim", 
+            hood.trackToAnglePoseBased(drivetrain, shooter)
+                .alongWith(
+                    shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
+                    intakePivot.jimmy(0.5),
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true))
+                .withTimeout(0.5));
+
+        NamedCommands.registerCommand("auto shoot", 
+            hood.trackToAnglePoseBased(drivetrain, shooter)
+                .alongWith(
+                    shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
+                    spindexer.spinToShooter(), 
+                    intakePivot.jimmy(0.5),
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true)));
     }
 
     public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -56,7 +56,7 @@ public class ModuleConstants {
 
     // TODO Hold over from 2025, adjust?
     public static final int kDriveMotorStatorCurrentLimit = 90;
-    public static final int kDriveMotorSupplyCurrentLimit = 55;
+    public static final int kDriveMotorSupplyCurrentLimit = 40;
 
     // TODO Hold over from 2025, adjust?
     public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -199,7 +199,7 @@ public class Drivetrain extends SubsystemBase {
      * @return A complete Command structure that performs the specified action
      */
     public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
-        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
+        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180, false)
             .until(yawRotationController::atSetpoint);
     }
 
@@ -218,14 +218,16 @@ public class Drivetrain extends SubsystemBase {
      * @param ySpeed The Y (left/right) translational speed of the robot
      * @param rotate180 When false, the front of the robot faces the hub, when true, the back
      *   of the robot faces the hub
+     * @param lockXWhenStopped When not being commanded to drive, the drivetrain wheels will make an X shape if this is true
      * @return A complete Command structure that performs the specified action
      */
-    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180, boolean lockXWhenStopped) {
         return lockRotationToSuppliedPose(
             Utilities::getHubPose, 
             xSpeed, 
             ySpeed, 
-            rotate180
+            rotate180,
+            lockXWhenStopped
         );
     }
 
@@ -243,9 +245,10 @@ public class Drivetrain extends SubsystemBase {
      * @param ySpeed The Y (left/right) translational speed of the robot
      * @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
      *   of the robot faces the supplied pose
+     * @param lockXWhenStopped When not being commanded to drive, the drivetrain wheels will make an X shape if this is true
      * @return A complete Command structure that performs the specified action
      */
-    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180, boolean lockXWhenStopped) {
         return runOnce(yawRotationController::reset).andThen(
             drive(
                 xSpeed,
@@ -274,7 +277,8 @@ public class Drivetrain extends SubsystemBase {
 
                     return outputPower;
                 }, 
-                () -> true
+                () -> true,
+                () -> lockXWhenStopped
             )
         );
     }
@@ -288,15 +292,17 @@ public class Drivetrain extends SubsystemBase {
      * @param yaw The "yaw" of the tag source relative to the center of the image frame
      * @param xSpeed The X (forward/backward) translational speed of the robot
      * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param lockXWhenStopped When not being commanded to drive, the drivetrain wheels will make an X shape if this is true
      * @return A complete Command structure that performs the specified action
      */
-    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
+    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean lockXWhenStopped) {
         return runOnce(yawRotationController::reset).andThen(
             drive(
                 xSpeed,
                 ySpeed, 
                 () -> yawRotationController.calculate(yaw.getAsDouble(), 0),
-                () -> true
+                () -> true,
+                () -> lockXWhenStopped
             )
         );
     }
@@ -310,14 +316,14 @@ public class Drivetrain extends SubsystemBase {
         
     }
 
-    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
-        // TODO Specific Alliance code?
+    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative, BooleanSupplier lockXWhenStopped) {
         return run(() -> {
             drive(
                 -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
                 -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
                 -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
-                fieldRelative.getAsBoolean()
+                fieldRelative.getAsBoolean(),
+                lockXWhenStopped.getAsBoolean()
             );
         });
     }
@@ -369,30 +375,41 @@ public class Drivetrain extends SubsystemBase {
         );
     }
 
-    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
-        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
-        double xSpeedDelivered = 0;
-        double ySpeedDelivered = 0;
+    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative, boolean lockXWhenStopped) {
+        if(lockXWhenStopped && 
+            MathUtil.applyDeadband(xSpeed, OIConstants.kDriveDeadband) == 0 &&
+            MathUtil.applyDeadband(ySpeed, OIConstants.kDriveDeadband) == 0 &&
+            MathUtil.applyDeadband(rotation, OIConstants.kDriveDeadband) == 0) {
 
-        if(p != 0){
-            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
-            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
-        }else{
-            xSpeedDelivered = 0;
-            ySpeedDelivered = 0;
+            frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+            frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+            rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+            rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+        } else {
+            double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+            double xSpeedDelivered = 0;
+            double ySpeedDelivered = 0;
+
+            if(p != 0){
+                xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+                ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            }else{
+                xSpeedDelivered = 0;
+                ySpeedDelivered = 0;
+            }
+
+            double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
+
+            SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
+                fieldRelative ?
+                    ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
+                        //estimator.getEstimatedPosition().getRotation()) :
+                        Rotation2d.fromDegrees(getGyroValue())) :
+                    new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
+            );
+
+            setModuleStates(swerveModuleStates);
         }
-
-        double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
-
-        SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
-            fieldRelative ?
-                ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
-                    //estimator.getEstimatedPosition().getRotation()) :
-                    Rotation2d.fromDegrees(getGyroValue())) :
-                new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
-        );
-
-        setModuleStates(swerveModuleStates);
     }
 
     public void driveWithChassisSpeeds(ChassisSpeeds speeds) {

src/main/java/frc/robot/subsystems/Spindexer.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.PersistMode;
@@ -44,6 +46,22 @@ public class Spindexer extends SubsystemBase {
         });
     }
 
+    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) {
+        return run(() -> {
+            if(shooterSpeedRPM.getAsDouble() < cutOffRPM) {
+                spindexerMotor.setControl(
+                    spindexerMotorOutput.withOutput(0)
+                );
+                feederMotor.set(0);
+            } else {
+                spindexerMotor.setControl(
+                    spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
+                );
+                feederMotor.set(SpindexerConstants.kFeederSpeed);
+            }
+        });
+    }
+
     public Command spinToIntake() {
         return run(() -> {
             spindexerMotor.setControl(