Robot should be ready

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

@@ -1,6 +1,9 @@
 package frc4388.robot.subsystems.intake;
 
 import static edu.wpi.first.units.Units.Rotation;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
 
 import java.util.function.Supplier;
 
@@ -12,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.utility.status.FaultReporter;
 
 public class Intake extends SubsystemBase {
-    IntakeIO io;
+    public IntakeIO io;
     IntakeStateAutoLogged state = new IntakeStateAutoLogged();
 
     Supplier<Pose2d> m_swervePoseSupplier;
@@ -26,19 +29,19 @@ public class Intake extends SubsystemBase {
     }
 
     public enum IntakeMode {
-        Up,
-        Down,
+        Extended,
+        Retracted,
     }
 
     public void setMode(IntakeMode mode) {
         switch (mode) {
-            case Up:
-                io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
-                io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
+            case Extended:
+                io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
+                io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
                 break;
-            case Down:
-                io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
-                io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
+            case Retracted:
+                io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
+                io.setRollerVelocity(state, RotationsPerSecond.of(0));
                 break;
         }
     }

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import frc4388.utility.configurable.ConfigurableDouble;
 import frc4388.utility.status.CanDevice;
 
 public class IntakeConstants {
@@ -33,21 +34,28 @@ public class IntakeConstants {
 
     //when testing the negative output of 10% made the robot put the intake up
 
-    public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
-    public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
-    public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
-    public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
+    // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
+    // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
+
+    public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0);
+    public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.25);
+    
+    public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
+    // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
+
+    // public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
+    // public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
 
 
     public static final Slot0Configs ARM_PID = new Slot0Configs()
-        .withKP(2.0)
+        .withKP(0.2)
         .withKI(0.0)
-        .withKD(10.0);
+        .withKD(0.0);
 
     public static final Slot0Configs ROLLER_PID = new Slot0Configs()
-        .withKP(2.0)
+        .withKP(0.2)
         .withKI(0.0)
-        .withKD(10.0);
+        .withKD(0.0);
 
     // 0 is paralell to the ground, 90 is directly up
     // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);

src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java

@@ -39,4 +39,5 @@ public interface IntakeIO {
     public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
 
     public default void updateInputs(IntakeState state) {}
+    public default void updateGains() {}
 }

src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

@@ -95,6 +95,10 @@ public class IntakeReal implements IntakeIO {
 
         state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
         state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
+    }
+
+    @Override 
+    public void updateGains() {
 
         IntakeConstants.ARM_PID.kP = arm_kP.get();
         IntakeConstants.ARM_PID.kI = arm_kI.get();