pid position tuning

src/main/java/frc/robot/Constants.java

@@ -164,7 +164,7 @@ public static final class ShooterConstants {
     public static final int rightShooterCanId = 13;
     public static final int leftShooterCanId = 16;
     public static final int ampSpeed = 1150;
-    public static final int reverseAmpSpeed = -1150;
+    public static final int reverseSpeed = -1150;
   }
 
   // intake constants

src/main/java/frc/robot/Robot.java

@@ -574,6 +574,7 @@ public void teleopPeriodic() {}
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+    drive.moveToPosition().schedule();
   }
 
   /** This function is called periodically during test mode. */

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -48,6 +48,12 @@
 
 public class DriveSubsystem extends SubsystemBase implements AutoCloseable {
   private final SwerveIO io;
+  private final PIDController xController =
+      new PIDController(Constants.AutoConstants.pXController, 0, 0);
+  private final PIDController yController =
+      new PIDController(Constants.AutoConstants.pYController, 0, 0);
+  private final PIDController thetaController =
+      new PIDController(Constants.AutoConstants.pThetaController, 0, 0);
 
   // Odometry class for tracking robot pose
   private final SwerveDrivePoseEstimator poseEstimator;
@@ -62,7 +68,7 @@ public enum ReferenceFrame {
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem(SwerveIO io) {
     this.io = io;
-
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
     poseEstimator =
         new SwerveDrivePoseEstimator(
             DriveConstants.driveKinematics,
@@ -84,6 +90,9 @@ public DriveSubsystem(SwerveIO io) {
     SmartDashboard.putData("FR", io.frontRight());
     SmartDashboard.putData("RL", io.rearLeft());
     SmartDashboard.putData("RR", io.rearRight());
+    Shuffleboard.getTab("Drive").add("x controller", xController);
+    Shuffleboard.getTab("Drive").add("y controller", yController);
+    Shuffleboard.getTab("Drive").add("theta controller", thetaController);
 
     Shuffleboard.getTab("Drive")
         .add("zero heading", runOnce(this::zeroHeading).ignoringDisable(true));
@@ -220,9 +229,9 @@ public Command followChoreoTrajectory(String trajectoryName) {
         Choreo.choreoSwerveCommand(
             traj,
             this::getPose,
-            new PIDController(Constants.AutoConstants.pXController, 0, 0),
-            new PIDController(Constants.AutoConstants.pYController, 0, 0),
-            new PIDController(Constants.AutoConstants.pThetaController, 0, 0),
+            xController,
+            yController,
+            thetaController,
             this::drive,
             () -> {
               if (DriverStation.getAlliance().isEmpty()) return false;
@@ -441,4 +450,14 @@ public Command autoDriveDiagonalCommand() {
         })
         .withTimeout(6);
   }
+
+  public Command moveToPosition() {
+    return run(() -> {
+      var pose = getPose();
+      var xSpeed = MetersPerSecond.of(xController.calculate(pose.getX()));
+      var ySpeed = MetersPerSecond.of(yController.calculate(pose.getY()));
+      var thetaSpeed = RadiansPerSecond.of(thetaController.calculate(getHeading().getRadians()));
+      drive(xSpeed, ySpeed, thetaSpeed, ReferenceFrame.FIELD);
+    });
+  }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -59,8 +59,10 @@ public void spin() {
   }
 
   public void reverseSpin() {
-    rSparkPID.setReference(-1700, CANSparkMax.ControlType.kVelocity);
-    lSparkPID.setReference(-1700, CANSparkMax.ControlType.kVelocity);
+    rSparkPID.setReference(
+        Constants.ShooterConstants.reverseSpeed, CANSparkMax.ControlType.kVelocity);
+    lSparkPID.setReference(
+        Constants.ShooterConstants.reverseSpeed, CANSparkMax.ControlType.kVelocity);
   }
 
   public boolean atSpeakerSpeed() {