basically fine

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

@@ -37,7 +37,7 @@ public final class Constants {
   public static final double intakeV = 0.0020349;
   public static final double intakeA = 0.00018939;
 
-  public static final double armP = 10;
+  public static final double armP = 37.5;
   public static final double armI = 0;
   public static final double armD = 0;
   public static final double armS = 0;

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

@@ -14,7 +14,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.autos.WompWompKieran;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Arm;
@@ -110,13 +109,13 @@ public Robot() {
     m_intake.setDefaultCommand(m_superstructure.intake());
     m_shooter.setDefaultCommand(m_shooter.stop());
     m_index.setDefaultCommand(m_index.stop());
-    m_arm.setDefaultCommand(m_arm.goToSetpoint(Setpoint.kAmp));
+    m_arm.setDefaultCommand(m_arm.goToSetpoint(Setpoint.kIntake));
 
     new Trigger(() -> m_codriver.getLeftY() > 0.05)
         .whileTrue(m_arm.manualControl(m_codriver::getLeftY));
 
-    // m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
-    // m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
+    m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
+    m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
 
     m_codriver.a().onTrue(m_climber.climb());
     m_codriver.leftBumper().whileTrue(m_index.shoot());
@@ -145,10 +144,5 @@ public Robot() {
             });
     m_codriver.y().onTrue(m_superstructure.stop());
     m_codriver.b().whileTrue(m_superstructure.outake());
-
-    m_driver.y().onTrue(m_arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    m_driver.b().onTrue(m_arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    m_driver.a().onTrue(m_arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    m_driver.x().onTrue(m_arm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
   }
 }

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

@@ -102,15 +102,15 @@ public enum Setpoint {
   /** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
   public Arm() {
     m_followerMotor.follow(m_primaryMotor);
-    m_pid.setTolerance(0.1);
+    m_pid.setTolerance(0.05);
   }
 
   /** Achieves and maintains speed for the primary motor. */
   private Command achievePosition(double position) {
+    m_pid.setSetpoint(position);
     return run(
         () -> {
           m_lastPosition = position;
-          m_pid.setSetpoint(position);
           var pid_output = m_pid.calculate(getAngle());
           log_pid_output.append(pid_output);
           log_pid_setpoint.append(m_pid.getSetpoint());
@@ -138,7 +138,7 @@ public Command stop() {
    * @return a {@link Command} to get to the desired position.
    */
   public Command moveToPosition(double position) {
-    return defer(() -> achievePosition(position).until(m_pid::atSetpoint));
+    return defer(() -> achievePosition(position).until(m_pid::atSetpoint)).andThen(stop());
   }
 
   /**

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

@@ -21,6 +21,9 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.MutableMeasure;
@@ -46,6 +49,10 @@
  */
 @SuppressWarnings("PMD.SingularField")
 public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
+  private final NetworkTable driveStats = NetworkTableInstance.getDefault().getTable("Drive");
+  private final DoublePublisher error = driveStats.getDoubleTopic("Error").publish();
+  private final DoublePublisher output = driveStats.getDoubleTopic("Output").publish();
+
   private static final double kSimLoopPeriod = 0.005; // 5 ms
   private Notifier m_simNotifier;
   private double m_lastSimTime;