Minor comment fixes

src/main/deploy/pathplanner/autos/Left Shoot Outpost.auto

@@ -11,41 +11,39 @@
           }
         },
         {
-          "type": "parallel",
+          "type": "named",
           "data": {
-            "commands": [
-              {
-                "type": "path",
-                "data": {
-                  "pathName": "Start to Outpost"
-                }
-              }
-            ]
+            "name": "spinup"
           }
         },
         {
-          "type": "wait",
+          "type": "path",
           "data": {
-            "waitTime": 2.0
+            "pathName": "start to score left"
           }
         },
         {
-          "type": "parallel",
+          "type": "named",
           "data": {
-            "commands": [
-              {
-                "type": "path",
-                "data": {
-                  "pathName": "trough to shot"
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "spinup"
-                }
-              }
-            ]
+            "name": "shoot close"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left to Outpost"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop spindexer"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "trough to shot"
           }
         },
         {
@@ -57,13 +55,7 @@
         {
           "type": "named",
           "data": {
-            "name": "aim"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "auto shoot"
+            "name": "shoot N jimmy"
           }
         }
       ]

src/main/deploy/pathplanner/paths/Left to Outpost.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.925604060913706,
-        "y": 5.503695431472081
+        "x": 3.2515736040609142,
+        "y": 4.914375634517767
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.107905365777289,
-        "y": 5.923303972397348
+        "x": 2.4338749089244973,
+        "y": 5.333984175443034
       },
       "isLocked": false,
       "linkedName": "left close"
     },
     {
       "anchor": {
-        "x": 0.32339086294416286,
-        "y": 5.9825177664974625
+        "x": 0.77458883248731,
+        "y": 5.927269035532995
       },
       "prevControl": {
-        "x": 1.9265106731458264,
-        "y": 6.165424053567452
+        "x": 2.3777086426889733,
+        "y": 6.110175322602984
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/Start to outpost.path

@@ -3,35 +3,35 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.5922741116751276,
-        "y": 6.3692588832487305
+        "x": 3.6619856887266913,
+        "y": 2.2583184257605784
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.956913705583757,
-        "y": 6.19430456852792
+        "x": 2.4937567084046877,
+        "y": 1.3172450805011864
       },
       "isLocked": false,
-      "linkedName": "start left"
+      "linkedName": null
     },
     {
       "anchor": {
-        "x": 0.32339086294416286,
-        "y": 5.9825177664974625
+        "x": 0.7089624329216013,
+        "y": 0.668228980317108
       },
       "prevControl": {
-        "x": 1.3270761421319812,
-        "y": 5.964101522842641
+        "x": 1.6987119856944104,
+        "y": 1.0414132379199703
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "trough"
+      "linkedName": "Outpost"
     }
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.6910862504511138,
-      "rotationDegrees": -179.02889973265033
+      "waypointRelativePos": 0.7234468937875753,
+      "rotationDegrees": 180.0
     }
   ],
   "constraintZones": [],
@@ -39,14 +39,14 @@
   "eventMarkers": [
     {
       "name": "Intake Start",
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 0.4500000000000002,
       "endWaypointRelativePos": null,
       "command": null
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 0.75,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -54,13 +54,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 178.80651057601818
+    "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Right Outpost",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 180.0
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }

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

@@ -1,75 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.5922741116751276,
-        "y": 6.3692588832487305
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.9329746192893413,
-        "y": 6.3692588832487305
-      },
-      "isLocked": false,
-      "linkedName": "start left"
-    },
-    {
-      "anchor": {
-        "x": 4.577543147208122,
-        "y": 5.715482233502538
-      },
-      "prevControl": {
-        "x": 4.264467005076142,
-        "y": 5.715482233502538
-      },
-      "nextControl": {
-        "x": 5.120900364352579,
-        "y": 5.715482233502538
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.041634517766498,
-        "y": 6.3692588832487305
-      },
-      "prevControl": {
-        "x": 5.931137055837564,
-        "y": 5.65102538071066
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "over bump"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.8,
-      "rotationDegrees": -135.0
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 2.0,
-    "rotation": -129.95754893082906
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -90.0
-  },
-  "useDefaultConstraints": false
-}

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

@@ -3,29 +3,45 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.5922741116751276,
+        "x": 3.573857868020305,
         "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.085827411167513,
-        "y": 6.277177664974619
+        "x": 3.0213705583756347,
+        "y": 6.461340101522843
       },
       "isLocked": false,
       "linkedName": "start left"
     },
+    {
+      "anchor": {
+        "x": 3.0766192893401025,
+        "y": 5.936477157360406
+      },
+      "prevControl": {
+        "x": 2.8556243654822344,
+        "y": 6.28638578680203
+      },
+      "nextControl": {
+        "x": 3.3374096167812595,
+        "y": 5.523559138911907
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
     {
       "anchor": {
         "x": 4.577543147208122,
         "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 3.2884060913705584,
-        "y": 5.697065989847716
+        "x": 3.5645859798769006,
+        "y": 5.732650999050524
       },
       "nextControl": {
-        "x": 5.120844928223563,
-        "y": 5.723243687517044
+        "x": 5.120822335025381,
+        "y": 5.706274111675127
       },
       "isLocked": false,
       "linkedName": null
@@ -46,7 +62,7 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.8,
+      "waypointRelativePos": 1.62272727272727,
       "rotationDegrees": -135.0
     }
   ],
@@ -55,7 +71,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

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

@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.5922741116751276,
+        "x": 3.573857868020305,
         "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.817612189966942,
+        "x": 2.7991959463121194,
         "y": 6.260219737341258
       },
       "isLocked": false,
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.925604060913706,
-        "y": 5.503695431472081
+        "x": 3.2515736040609142,
+        "y": 4.914375634517767
       },
       "prevControl": {
-        "x": 2.119269541340752,
-        "y": 6.020136712610872
+        "x": 3.44523908448796,
+        "y": 5.430816915656558
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/trough to shot.path

@@ -3,43 +3,38 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.32339086294416286,
-        "y": 5.9825177664974625
+        "x": 0.77458883248731,
+        "y": 5.927269035532995
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.6364670050761427,
-        "y": 6.03776649746193
+        "x": 1.7745888324873098,
+        "y": 5.927269035532995
       },
       "isLocked": false,
       "linkedName": "trough"
     },
     {
       "anchor": {
-        "x": 1.925604060913706,
-        "y": 5.503695431472081
+        "x": 3.2515736040609142,
+        "y": 4.914375634517767
       },
       "prevControl": {
-        "x": 1.409949238578681,
-        "y": 5.844395939086294
+        "x": 2.6254213197969554,
+        "y": 5.420822335025381
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": "left close"
     }
   ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.7098520389751093,
-      "rotationDegrees": 176.84552599629856
-    }
-  ],
+  "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -55,5 +50,5 @@
     "velocity": 0,
     "rotation": 178.80651057601818
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }

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

@@ -32,8 +32,6 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
-import edu.wpi.first.wpilibj2.command.Subsystem;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -44,14 +42,12 @@ import frc.robot.constants.OIConstants;
 import frc.robot.constants.ShooterConstants;
 import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
 import frc.robot.constants.ShooterConstants.ShooterSpeeds;
-import frc.robot.interfaces.IIndexer;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Hood;
 import frc.robot.subsystems.IntakePivot;
 import frc.robot.subsystems.IntakeRoller;
 import frc.robot.subsystems.PhotonVision;
-import frc.robot.subsystems.RollerIndexer;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Spindexer;
 import frc.robot.utilities.Elastic;
@@ -64,7 +60,7 @@ public class RobotContainer {
     private Shooter shooter;
     private IntakePivot intakePivot;
     private IntakeRoller intakeRoller;
-    private IIndexer indexer;
+    private Spindexer spindexer;
     //private Climber climber;
 
     private CommandXboxController driver;
@@ -83,7 +79,7 @@ public class RobotContainer {
         shooter = new Shooter();
         intakePivot = new IntakePivot();
         intakeRoller = new IntakeRoller();
-        indexer = new RollerIndexer();
+        spindexer = new Spindexer();
         //climber = new Climber();
         configureNamedCommands();
 
@@ -152,7 +148,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
 
@@ -188,7 +185,7 @@ public class RobotContainer {
         intakePivot.setDefaultCommand(intakePivot.stop());
         intakeRoller.setDefaultCommand(intakeRoller.stop());
         hood.setDefaultCommand(hood.stop());
-        ((SubsystemBase)indexer).setDefaultCommand(indexer.stop());
+        spindexer.setDefaultCommand(spindexer.stop());
         //climber.setDefaultCommand(climber.stop());
 
         // While holding POV up of the driver controller, the climber
@@ -202,23 +199,23 @@ public class RobotContainer {
         //}));
 
         // While holding the right bumper of the driver controller, the intake rollers
-        // and the indexer and feeder should move such that all motors are moving in such a way
-        // that it would draw balls from the floor, through the indexer, and into the
+        // and the spindexer and feeder should move such that all motors are moving in such a way
+        // that it would draw balls from the floor, through the spindexer, and into the
         // feeder. 
         // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
         // constants file for the subsystem having the problem
         driver.rightBumper().whileTrue(
-            indexer.spinToShooter()  
+            spindexer.spinToShooter()  
         );
 
         // While holding the left bumper of the driver controller, the intake rollers
-        // and the indexer and feeder should move such that all motors are moving in such a way
+        // and the spindexer and feeder should move such that all motors are moving in such a way
         // that it would try to eject balls through the intake.
         // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
         // constants file for the subsystem having the problem
         driver.leftBumper().whileTrue(
             intakeRoller.runIn()
-            //intakeRoller.runOut().alongWith(indexer.spinToIntake())
+            //intakeRoller.runOut().alongWith(spindexer.spinToIntake())
         );
 
         // While holding D-Pad up on the secondary controller, the shooter should spin
@@ -280,12 +277,13 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
         shooter.setDefaultCommand(shooter.stop());
         intakeRoller.setDefaultCommand(intakeRoller.stop());
-        ((SubsystemBase)indexer).setDefaultCommand(indexer.stop());
+        spindexer.setDefaultCommand(spindexer.stop());
 
         intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
 
@@ -299,7 +297,8 @@ public class RobotContainer {
             drivetrain.lockRotationToHub(
                 driver::getLeftY, 
                 driver::getLeftX, 
-                false
+                false,
+                true
             ) 
         );
 
@@ -307,7 +306,7 @@ public class RobotContainer {
         driver.rightBumper().whileTrue(intakeRoller.runIn());
 
         driver.rightTrigger().whileTrue(
-            indexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
+            spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
                 intakeRoller.runIn()/* ,
                 intakePivot.jimmy(.5)*/
             )
@@ -322,7 +321,7 @@ public class RobotContainer {
             intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
         );
 
-        driver.b().whileTrue(indexer.spinToIntake());
+        driver.b().whileTrue(spindexer.spinToIntake());
        /*  driver.b().whileTrue(
             drivetrain.lockToYaw(
                 () -> {
@@ -416,10 +415,10 @@ public class RobotContainer {
                 .withTimeout(2));
 
         NamedCommands.registerCommand("shoot close", 
-            indexer.spinToShooter()
+            spindexer.spinToShooter()
                 .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed))
                 .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10)))
-                .withTimeout(3).andThen(indexer.instantaneousStop()));
+                .withTimeout(3).andThen(spindexer.instantaneousStop()));
         
        // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
 
@@ -431,7 +430,7 @@ public class RobotContainer {
             .onTrue(
                 shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
 
-        NamedCommands.registerCommand("stop spindexer", indexer.instantaneousStop());
+        NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
 
         NamedCommands.registerCommand("jimmy",
             intakePivot.jimmy(0.2)
@@ -440,27 +439,27 @@ public class RobotContainer {
         NamedCommands.registerCommand("shoot N jimmy", 
             Commands.parallel(
                 intakePivot.jimmy(0.5),
-                indexer.spinToShooter()
+                spindexer.spinToShooter()
                     .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
                                hood.trackToAngle(() -> Units.degreesToRadians(10)))
                 
-            ).withTimeout(3).andThen(indexer.instantaneousStop()));
+            ).withTimeout(3).andThen(spindexer.instantaneousStop()));
 
         NamedCommands.registerCommand("aim", 
             hood.trackToAnglePoseBased(drivetrain, shooter)
                 .alongWith(
                     shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
                     intakePivot.jimmy(0.5),
-                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false))
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true))
                 .withTimeout(0.5));
 
         NamedCommands.registerCommand("auto shoot", 
             hood.trackToAnglePoseBased(drivetrain, shooter)
                 .alongWith(
                     shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
-                    indexer.spinToShooter(), 
+                    spindexer.spinToShooter(), 
                     intakePivot.jimmy(0.5),
-                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false)));
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true)));
     }
 
     public Command getAutonomousCommand() {

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

@@ -1,28 +0,0 @@
-package frc.robot.constants;
-
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-public class RollerIndexerConstants {
-    public static final int kMotor1CANID = 50;
-    public static final int kMotor2CANID = 4;
-
-    public static final int kCurrentLimit = 40;
-
-    public static final double kMotorSpeed = 1;
-
-    public static final boolean kMotorInverted = true;
-
-    public static final IdleMode kIdleMode = IdleMode.kBrake;
-
-    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
-
-    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
-    
-    static {
-        kMotorConfig
-            .inverted(kMotorInverted)
-            .smartCurrentLimit(kCurrentLimit)
-            .idleMode(kIdleMode);
-    }
-}

src/main/java/frc/robot/interfaces/IIndexer.java

@@ -1,13 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-public interface IIndexer {
-    public Command spinToShooter();
-    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM);
-    public Command spinToIntake();
-    public Command stop();
-    public Command instantaneousStop();
-}

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/IntakeRoller.java

@@ -32,8 +32,8 @@ public class IntakeRoller extends SubsystemBase {
 
     public Command runIn() {
         return run(() -> {
-            leftMotor.set(IntakeRollerConstants.kSpeed*0.9);
-            rightMotor.set(IntakeRollerConstants.kSpeed*0.9);
+            leftMotor.set(IntakeRollerConstants.kSpeed*0.8);
+            rightMotor.set(IntakeRollerConstants.kSpeed*0.8);
         });
     }
 

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

@@ -1,78 +0,0 @@
-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;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.RollerIndexerConstants;
-import frc.robot.interfaces.IIndexer;
-
-public class RollerIndexer extends SubsystemBase implements IIndexer {
-    private SparkMax motor1;
-    private SparkMax motor2;
-
-    public RollerIndexer() {
-        motor1 = new SparkMax(RollerIndexerConstants.kMotor1CANID, MotorType.kBrushless);
-        motor2 = new SparkMax(RollerIndexerConstants.kMotor2CANID, MotorType.kBrushless);
-
-        motor1.configure(
-            RollerIndexerConstants.kMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        motor2.configure(
-            RollerIndexerConstants.kMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-    }
-
-    public Command spinToShooter() {
-        return run(() -> {
-            motor1.set(RollerIndexerConstants.kMotorSpeed);
-            motor2.set(-RollerIndexerConstants.kMotorSpeed);
-        });
-    }
-
-    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) {
-        return run(() -> {
-            if(shooterSpeedRPM.getAsDouble() < cutOffRPM) {
-                motor1.set(0);
-                motor2.set(0);
-            } else {
-                motor1.set(RollerIndexerConstants.kMotorSpeed);
-                motor2.set(-RollerIndexerConstants.kMotorSpeed);
-            }
-        });
-    }
-
-    public Command spinToIntake() {
-        return run(() -> {
-            motor1.set(-RollerIndexerConstants.kMotorSpeed);
-            motor2.set(RollerIndexerConstants.kMotorSpeed);
-        });
-    }
-
-    public Command stop() {
-        return run(() -> {
-            motor1.set(0);
-            motor2.set(0);
-        });
-    }
-
-    public Command instantaneousStop() {
-        return runOnce(() -> {
-            motor1.set(0);
-            motor2.set(0);
-        });
-    }
-    
-}

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

@@ -12,9 +12,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.SpindexerConstants;
-import frc.robot.interfaces.IIndexer;
 
-public class Spindexer extends SubsystemBase implements IIndexer {
+public class Spindexer extends SubsystemBase {
     private TalonFX spindexerMotor;
 
     private SparkMax feederMotor;