setup tunable numbers

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

@@ -74,8 +74,6 @@ public Elevator(ElevatorIO elevator) {
     extenderProfile = new TrapezoidProfile(extenderConstraints);
     extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
 
-    extenderkP.initDefault(15);
-
     this.elevator.configurePID(extenderkP.get(), 0, 0);
   }
 
@@ -135,7 +133,5 @@ public void periodic() {
     if (extenderkP.hasChanged(hashCode())) {
       elevator.configurePID(extenderkP.get(), 0, 0);
     }
-
-
   }
 }

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
+import frc.robot.util.LoggedTunableNumber;
 
 import static edu.wpi.first.units.Units.Volts;
 
@@ -17,14 +18,9 @@
 public class Shooter extends SubsystemBase {
   /** Creates a new Shooter. */
   private final FlywheelIO flywheels;
-
-  // private final FlywheelIO shooterMotor2;
-
   private final FeederIO feeder;
 
   private final FlywheelIOInputsAutoLogged fInputs = new FlywheelIOInputsAutoLogged();
-  // private final FlywheelIOInputsAutoLogged s2Inputs = new FlywheelIOInputsAutoLogged();
-
   private final FeederIOInputsAutoLogged feedInputs = new FeederIOInputsAutoLogged();
 
   private final SimpleMotorFeedforward flywheelFFModel;
@@ -33,11 +29,17 @@ public class Shooter extends SubsystemBase {
   private final SysIdRoutine feedSysId;
   private final SysIdRoutine flywheelSysId;
 
+  private static final LoggedTunableNumber feederkP = new LoggedTunableNumber("feederkP");
+  private static final LoggedTunableNumber flywheelkP = new LoggedTunableNumber("flywheelkP");
+
   public Shooter(FlywheelIO flywheels, FeederIO feeder) {
     switch (Constants.currentMode) {
       case REAL:
         flywheelFFModel = new SimpleMotorFeedforward(0, 3);
         feederFFModel = new SimpleMotorFeedforward(0, 0.3);
+
+        flywheelkP.initDefault(0);
+        feederkP.initDefault(0);
         break;
       case REPLAY:
         flywheelFFModel = new SimpleMotorFeedforward(0, 0.03);
@@ -54,7 +56,7 @@ public Shooter(FlywheelIO flywheels, FeederIO feeder) {
     }
     this.flywheels = flywheels;
     // TODO:: Make these constants
-    flywheels.configurePID(0.5, 0, 0);
+    flywheels.configurePID(flywheelkP.get(), 0, 0);
 
     // Configure SysId
     flywheelSysId =
@@ -74,7 +76,7 @@ public Shooter(FlywheelIO flywheels, FeederIO feeder) {
 
     this.feeder = feeder;
     // TODO:: Make these constants
-    feeder.configurePID(0.5, 0, 0);
+    feeder.configurePID(feederkP.get(), 0, 0);
 
     // Configure SysId
     feedSysId =
@@ -137,12 +139,12 @@ public Command feedSysIDDynamiCommand(SysIdRoutine.Direction direction) {
 
     /** Returns a command to run a quasistatic test in the specified direction. */
   public Command flywheelSysIDQuasistic(SysIdRoutine.Direction direction) {
-    return feedSysId.quasistatic(direction);
+    return flywheelSysId.quasistatic(direction);
   }
 
   /** Returns a command to run a dynamic test in the specified direction. */
   public Command flywheelSysIDDynamiCommand(SysIdRoutine.Direction direction) {
-    return feedSysId.dynamic(direction);
+    return flywheelSysId.dynamic(direction);
   }
 
   @Override
@@ -154,5 +156,13 @@ public void periodic() {
 
     Logger.processInputs("Flywheels", fInputs);
     Logger.processInputs("Feeder", feedInputs);
+
+    if (feederkP.hasChanged(hashCode())) {
+      feeder.configurePID(feederkP.get(), 0, 0);
+    }
+
+    if (flywheelkP.hasChanged(hashCode())) {
+      flywheels.configurePID(flywheelkP.get(), 0, 0);
+    }
   }
 }

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -21,7 +21,6 @@ public class Intake extends SubsystemBase {
   private final IntakeRollerIOInputsAutoLogged rInputs = new IntakeRollerIOInputsAutoLogged();
 
   private static final LoggedTunableNumber intakekP = new LoggedTunableNumber("intake kP");
-  private static final LoggedTunableNumber intakekD = new LoggedTunableNumber("intake kD");
 
   private final SimpleMotorFeedforward ffModel;
 
@@ -32,7 +31,6 @@ public Intake(IntakeRollerIO roller) {
       case REAL:
         ffModel = new SimpleMotorFeedforward(0, 0.003);
         intakekP.initDefault(0.0000000004);
-        intakekD.initDefault(0.007);
         break;
       case REPLAY:
         ffModel = new SimpleMotorFeedforward(0, 0);
@@ -89,8 +87,8 @@ public void periodic() {
     // This method will be called once per scheduler run
     roller.updateInputs(rInputs);
 
-    if (intakekP.hasChanged(hashCode()) || intakekD.hasChanged(hashCode())) {
-      roller.configurePID(intakekP.get(), 0, intakekD.get());
+    if (intakekP.hasChanged(hashCode())) {
+      roller.configurePID(intakekP.get(), 0, 0);
     }
 
     Logger.processInputs("Intake", rInputs);

src/main/java/frc/robot/subsystems/pivot/Pivot.java

@@ -75,6 +75,8 @@ public Pivot(PivotIO pivot) {
     pivotProfile = new TrapezoidProfile(pivotConstraints);
 
     pivotCurrent = pivotProfile.calculate(0, pivotCurrent, pivotGoal);
+
+    pivot.configurePID(pivotkP.get(), 0, 0);
   }
 
   public double getPivotPosition() {