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

@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "over bump to pile move"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "back from center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -5,15 +5,72 @@
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "intake down"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
           }
         },
         {
-          "type": "path",
+          "type": "parallel",
           "data": {
-            "pathName": "left start to center"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "over bump to pile"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "back from center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
           }
         }
       ]

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

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.68848717948718,
+        "y": 5.639692307692307
+      },
+      "isLocked": false,
+      "linkedName": "after center grab"
+    },
+    {
+      "anchor": {
+        "x": 2.979166666666668,
+        "y": 5.2327051282051285
+      },
+      "prevControl": {
+        "x": 3.0721923076923088,
+        "y": 5.721089743589744
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.11569148936170148,
+      "rotationDegrees": -34.71279786419313
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -36.158185439808385
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
+  },
+  "useDefaultConstraints": false
+}

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

@@ -8,119 +8,78 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.1594923857868027,
-        "y": 6.424507614213196
+        "x": 3.0213705583756347,
+        "y": 6.461340101522843
       },
       "isLocked": false,
       "linkedName": "start left"
     },
     {
       "anchor": {
-        "x": 2.634629441624366,
-        "y": 5.558944162436549
+        "x": 3.0766192893401025,
+        "y": 5.936477157360406
       },
       "prevControl": {
-        "x": 2.4136345177664977,
-        "y": 5.908852791878173
+        "x": 2.8556243654822344,
+        "y": 6.28638578680203
       },
       "nextControl": {
-        "x": 2.895419769065523,
-        "y": 5.146026143988051
+        "x": 3.3374096167812595,
+        "y": 5.523559138911907
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.522294416243654,
-        "y": 5.558944162436549
+        "x": 4.577543147208122,
+        "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 3.5095162657293457,
-        "y": 5.533304209258972
+        "x": 3.5645859798769006,
+        "y": 5.732650999050524
       },
       "nextControl": {
-        "x": 6.704619289340102,
-        "y": 5.614192893401015
+        "x": 5.120822335025381,
+        "y": 5.706274111675127
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.478101522849547,
-        "y": 7.244030456855094
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": {
-        "x": 6.765542310662505,
-        "y": 7.465169522706244
-      },
-      "nextControl": {
-        "x": 8.012172588839395,
-        "y": 7.078284263961693
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.671472081218274,
-        "y": 4.7670456852791885
-      },
-      "prevControl": {
-        "x": 7.726720812182741,
-        "y": 7.639979695431472
+        "x": 5.931137055837564,
+        "y": 5.65102538071066
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "over bump"
     }
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 1.5636363636363326,
-      "rotationDegrees": -45.0
-    },
-    {
-      "waypointRelativePos": 3.0318181818182266,
-      "rotationDegrees": -90.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 2.6208291203235685,
-      "maxWaypointRelativePos": 4.0,
-      "constraints": {
-        "maxVelocity": 1.0,
-        "maxAcceleration": 1.5,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
+      "waypointRelativePos": 1.62272727272727,
+      "rotationDegrees": -135.0
     }
   ],
+  "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "Intake Start",
-      "waypointRelativePos": 0,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
+  "eventMarkers": [],
   "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": -89.09061955080092
+    "velocity": 2.0,
+    "rotation": -129.95754893082906
   },
   "reversed": false,
   "folder": null,
@@ -128,5 +87,5 @@
     "velocity": 0,
     "rotation": -90.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -0,0 +1,116 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
+      },
+      "isLocked": false,
+      "linkedName": "over bump"
+    },
+    {
+      "anchor": {
+        "x": 7.2386903553299495,
+        "y": 7.326903553299493
+      },
+      "prevControl": {
+        "x": 6.2386903553299495,
+        "y": 7.326903553299493
+      },
+      "nextControl": {
+        "x": 8.238690355329947,
+        "y": 7.326903553299493
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.417329949238578,
+        "y": 4.831502538071066
+      },
+      "prevControl": {
+        "x": 8.475340101522843,
+        "y": 6.042370558375635
+      },
+      "nextControl": {
+        "x": 8.359319796954313,
+        "y": 3.620634517766498
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
+      },
+      "prevControl": {
+        "x": 7.758358974358975,
+        "y": 5.616435897435898
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "after center grab"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0045454545454569,
+      "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.1243680485338854,
+      "maxWaypointRelativePos": 2.077102803738317,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.7886754297269942,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -129.95754893082906
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,116 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
+      },
+      "isLocked": false,
+      "linkedName": "over bump"
+    },
+    {
+      "anchor": {
+        "x": 7.220274111675127,
+        "y": 7.299279187817259
+      },
+      "prevControl": {
+        "x": 6.220274111675127,
+        "y": 7.299279187817259
+      },
+      "nextControl": {
+        "x": 8.220274111675131,
+        "y": 7.299279187817259
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.883258883248731,
+        "y": 4.831502538071066
+      },
+      "prevControl": {
+        "x": 7.952417879560978,
+        "y": 6.041784973535379
+      },
+      "nextControl": {
+        "x": 7.846426395939085,
+        "y": 4.186934010152284
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
+      },
+      "prevControl": {
+        "x": 7.598549873246986,
+        "y": 5.589703440094898
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "after center grab"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0045454545454569,
+      "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.3403967538322836,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.7886754297269942,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -129.95754893082906
+  },
+  "useDefaultConstraints": false
+}

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 
 import java.util.Optional;
 import java.util.OptionalDouble;
@@ -13,6 +14,7 @@ import org.littletonrobotics.junction.Logger;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.events.EventTrigger;
 import com.pathplanner.lib.path.EventMarker;
 
@@ -27,6 +29,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 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.button.CommandXboxController;
@@ -82,13 +85,14 @@ public class RobotContainer {
 
         resetOdometryToVisualPose = false;
          
-        //vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+        vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
                 "Vision/" + vp.cameraName() + "/Pose", 
                 vp.visualPose()
             );
         });
+
         vision.addPoseEstimateConsumer((vp) -> {
             if(resetOdometryToVisualPose) {
                 drivetrain.resetOdometry(vp.visualPose());
@@ -96,7 +100,6 @@ public class RobotContainer {
             }
         });
         
-
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
@@ -104,12 +107,23 @@ public class RobotContainer {
         shiftTimer.reset();
 
         configureBindings();
-        //testConfigureBindings();
         configureShiftDisplay();
+        //testConfigureBindings();
+        
 
         if(AutoConstants.kAutoConfigOk) {
             autoChooser = AutoBuilder.buildAutoChooser();
             SmartDashboard.putData("Auto Chooser", autoChooser);
+
+            autoChooser.addOption(
+                "MOVE B____ right to center", 
+                new PathPlannerAuto("MOVE B____ left to center", true)
+            );
+
+            autoChooser.addOption(
+                "right to center", 
+                new PathPlannerAuto("left to center", true)
+            );
         }
     }
 
@@ -134,7 +148,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
 
@@ -240,7 +255,7 @@ public class RobotContainer {
 
         // Useful for testing PID and FF responses of the shooter
         // You need to have graphs up of the logged data to make sure the response is correct
-        secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+        //secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
         secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
 
         // Useful for testing PID and FF responses of the intake pivot
@@ -262,7 +277,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
         shooter.setDefaultCommand(shooter.stop());
@@ -271,21 +287,40 @@ 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());
 
         driver.leftTrigger().whileTrue(
             drivetrain.lockRotationToHub(
                 driver::getLeftY, 
                 driver::getLeftX, 
-                false
+                false,
+                true
             ) 
         );
 
         driver.leftBumper().whileTrue(intakeRoller.runOut());
         driver.rightBumper().whileTrue(intakeRoller.runIn());
 
-        driver.rightTrigger().whileTrue(spindexer.spinToShooter());
+        driver.rightTrigger().whileTrue(
+            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)
+        );
+
         driver.b().whileTrue(spindexer.spinToIntake());
        /*  driver.b().whileTrue(
             drivetrain.lockToYaw(
@@ -307,11 +342,12 @@ public class RobotContainer {
         //40 good for feeding
         //secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
         //30 degrees good for shooter far near outpost
-        secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
+        secondary.rightBumper().whileTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
         //10 degrees good for shooting ~33in away from hub
 
+        hood.setDefaultCommand(hood.trackToAnglePoseBased(drivetrain, shooter));
         
-        hood.setDefaultCommand(hood.trackToAngle(() -> {
+        /*hood.setDefaultCommand(hood.trackToAngle(() -> {
             Pose2d drivetrainPose = drivetrain.getPose();
             Pose2d hubPose = Utilities.getHubPose();
 
@@ -333,7 +369,25 @@ public class RobotContainer {
             } else {
                 return 0;
             }
-        }));
+        }));*/
+
+        new Trigger(() -> MathUtil.isNear(
+            shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), 
+            shooter.getAverageActualSpeeds(), 
+            150)).onTrue(
+            new FunctionalCommand(
+                () -> {}, 
+                () -> {
+                    driver.setRumble(RumbleType.kBothRumble, .75);
+                    secondary.setRumble(RumbleType.kBothRumble, .75);
+                }, 
+                (b) -> {
+                    driver.setRumble(RumbleType.kBothRumble, 0);
+                    secondary.setRumble(RumbleType.kBothRumble, 0);
+                }, 
+                () -> false
+            ).withTimeout(1)
+        );
     }
 
     private void configureNamedCommands() {
@@ -358,7 +412,7 @@ public class RobotContainer {
 
         NamedCommands.registerCommand("spinup", 
             shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
-                .withTimeout(3));
+                .withTimeout(2));
 
         NamedCommands.registerCommand("shoot close", 
             spindexer.spinToShooter()
@@ -369,28 +423,43 @@ public class RobotContainer {
        // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
 
         new EventTrigger("Intake Start")
-            .onTrue(intakeRoller.runIn());
+            .onTrue(
+                intakeRoller.runIn());
+
+        new EventTrigger("windup trigger")
+            .onTrue(
+                shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
 
         NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
 
         NamedCommands.registerCommand("jimmy",
-            Commands.repeatingSequence(
-                    intakePivot.manualSpeed(() -> -0.75).withTimeout(0.2)
-                    .andThen(intakePivot.manualSpeed(() -> 0.75).withTimeout(0.2))
-                )
-            );
+            intakePivot.jimmy(0.2)
+        );
 
         NamedCommands.registerCommand("shoot N jimmy", 
             Commands.parallel(
-                Commands.repeatingSequence(
-                    intakePivot.manualSpeed(() -> -0.75).withTimeout(0.5),
-                    intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
-                ),
+                intakePivot.jimmy(0.5),
                 spindexer.spinToShooter()
                     .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
                                hood.trackToAngle(() -> Units.degreesToRadians(10)))
                 
             ).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, 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/DrivetrainConstants.java

@@ -46,7 +46,7 @@ public class DrivetrainConstants {
     // TODO How much do we trust gyro and encoders vs vision estimates.
     // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
     public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
-    public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.9, 0.9, 0.9);
+    public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.3, 0.3, 0.3);
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(

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

@@ -71,22 +71,22 @@ public class HoodConstants {
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
-            Double.valueOf(Units.degreesToRadians(10)));
+            Double.valueOf(Units.degreesToRadians(9.5)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
-            Double.valueOf(Units.degreesToRadians(13)));
+            Double.valueOf(Units.degreesToRadians(12.5)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 80)),
-            Double.valueOf(Units.degreesToRadians(17)));
+            Double.valueOf(Units.degreesToRadians(16.25)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
-            Double.valueOf(Units.degreesToRadians(21)));
+            Double.valueOf(Units.degreesToRadians(20.5)));
 
         kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
             Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
-            Double.valueOf(Units.degreesToRadians(24)));
+            Double.valueOf(Units.degreesToRadians(23.5)));
     }
 }

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/constants/ShooterConstants.java

@@ -1,11 +1,14 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.util.Units;
 
+
 public class ShooterConstants {
     public enum ShooterSpeeds {
         kHubSpeed(3000.0),
@@ -40,18 +43,18 @@ public class ShooterConstants {
     public static final boolean kLeftShooterMotorInverted = true;
     public static final boolean kRightShooterMotorInverted = false;
 
-    public static final double kLeftP = 0.001;
+    public static final double kLeftP = 0.75;//0.01;//0.001;
     public static final double kLeftI = 0;
-    public static final double kLeftD = 0;
+    public static final double kLeftD = 0;//0.1;//1.8;
     public static final double kLeftS = 0;
-    public static final double kLeftV = 0.0013;
+    public static final double kLeftV = 0.00129;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0.001;
+    public static final double kRightP = 0.75;//0.001;//0.001;
     public static final double kRightI = 0;
-    public static final double kRightD = 0.000;
+    public static final double kRightD = 0;//0.1;
     public static final double kRightS = 0;
-    public static final double kRightV = 0.00121;
+    public static final double kRightV = 0.00125;
     public static final double kRightA = 0;
 
     public static final double kMaxManualSpeedMultiplier = 1;
@@ -76,13 +79,15 @@ public class ShooterConstants {
             .inverted(kLeftShooterMotorInverted);
         kLeftMotorConfig.absoluteEncoder
             .positionConversionFactor(1)
-            .velocityConversionFactor(60);
+            .velocityConversionFactor(60)
+            .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
         kLeftMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kLeftP, kLeftI, kLeftD)
+            .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
-                .sva(kLeftS, kLeftV, kLeftA);
+                .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
 
         kRightMotorConfig
             .idleMode(kShooterIdleMode)
@@ -91,12 +96,14 @@ public class ShooterConstants {
         kRightMotorConfig.absoluteEncoder
             .positionConversionFactor(1)
             .velocityConversionFactor(60)
+            .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
             .inverted(true);
         kRightMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kRightP, kRightI, kRightD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
-                .sva(kRightS, kRightV, kRightA);
+                .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
     }
 }

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
@@ -12,12 +13,18 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.HoodConstants;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+import frc.robot.utilities.Utilities;
 
 public class Hood extends SubsystemBase {
     private SparkMax motor;
@@ -68,6 +75,21 @@ public class Hood extends SubsystemBase {
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber(
+            "HoodTargetDegrees", 
+            Math.toDegrees(currentTargetDegrees)
+        );
+
+        SmartDashboard.putNumber(
+            "HoodCurrentAngle", 
+            Math.toDegrees(encoder.getPosition())
+        );
+
+        SmartDashboard.putBoolean(
+            "HoodAtSetpoint", 
+            controller.isAtSetpoint()
+        );
+        
         Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
         Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
         Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
@@ -75,6 +97,32 @@ public class Hood extends SubsystemBase {
         Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
     }
 
+    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
+        return trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            double distance = drivetrainPose.getTranslation()
+                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        });
+    }
+
     public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
         return run(() -> {
             currentTargetDegrees = degreeAngleSupplier.getAsDouble();

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

@@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakePivotConstants;
 import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
@@ -80,12 +81,31 @@ public class IntakePivot extends SubsystemBase {
         });
     }
 
-    // public Command jimmy(){
-    //     return run(() -> {
-    //         leftMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier);
-    //         rightMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier);
-    //     })
-    // }
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(double time){
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time),
+            manualSpeed(() -> 0.75).withTimeout(time)
+        );
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(DoubleSupplier time) {
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()),
+            manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())
+        );
+    }
 
     public Command stop() {
         return manualSpeed(() -> 0);

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

@@ -9,11 +9,14 @@ import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ShooterConstants;
@@ -32,6 +35,8 @@ public class Shooter extends SubsystemBase {
     private SparkClosedLoopController rightClosedLoopController;
 
     private ShooterSpeeds targetSpeeds;
+    private SimpleMotorFeedforward shooterFFLeft;
+    private SimpleMotorFeedforward shooterFFRight;
 
     public Shooter() {
         leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
@@ -59,10 +64,37 @@ public class Shooter extends SubsystemBase {
         targetSpeeds = null;
 
         rightRelative = rightMotor.getEncoder();
+
+        shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA);
+        shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA);
     }
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber(
+            "ShooterTargetRPM", 
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM());
+
+        SmartDashboard.putNumber(
+            "ShooterLeftSideRPM", 
+            leftEncoder.getVelocity()
+        );
+
+        SmartDashboard.putNumber(
+            "ShooterRightSideRPM", 
+            rightEncoder.getVelocity()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterLeftSideAtSetpoint", 
+            leftClosedLoopController.isAtSetpoint()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterRightSideAtSetpoint", 
+            rightClosedLoopController.isAtSetpoint()
+        );
+
         Logger.recordOutput(
             "Shooter/TargetRPM", 
             targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()
@@ -73,6 +105,8 @@ public class Shooter extends SubsystemBase {
 
         Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity());
 
+        Logger.recordOutput("Shooter/LeftRollers/OutputVoltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage());
+        Logger.recordOutput("Shooter/RightRollers/OutputVoltage", rightMotor.getAppliedOutput() * rightMotor.getBusVoltage());
 
         // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
         Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
@@ -89,12 +123,16 @@ public class Shooter extends SubsystemBase {
             } else {
                 leftClosedLoopController.setSetpoint(
                     targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFLeft.calculate(targetSpeeds.getSpeedRPM())
                 );
 
                 rightClosedLoopController.setSetpoint(
                     targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFRight.calculate(targetSpeeds.getSpeedRPM())
                 );
             }
         });

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(