Get IDK to work

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

@@ -57,6 +57,15 @@ public class IntakeConstants {
         .withKI(0.0)
         .withKD(0.0);
 
+    public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2);
+    public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0);
+    public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0);
+    
+    public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2);
+    public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0);
+    public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0);
+
+
     // 0 is paralell to the ground, 90 is directly up
     // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
     // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);

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

@@ -58,6 +58,12 @@ public class IntakeReal implements IntakeIO {
     @Override
     public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {
         state.rollerTargetVelocity = angularVelocity;
+
+        if(angularVelocity.baseUnitMagnitude() == 0) {
+            m_rollerMotor.set(0);
+            return;
+        }
+
         // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
         AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
 
@@ -80,14 +86,6 @@ public class IntakeReal implements IntakeIO {
         m_armMotor.setControl(armPosition.withPosition(motorAngle));
     }
 
-    ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2);
-    ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0);
-    ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0);
-    
-    ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2);
-    ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0);
-    ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0);
-
     @Override
     public void updateInputs(IntakeState state) {
         state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
@@ -100,14 +98,14 @@ public class IntakeReal implements IntakeIO {
     @Override 
     public void updateGains() {
 
-        IntakeConstants.ARM_PID.kP = arm_kP.get();
+        IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get();
-        IntakeConstants.ARM_PID.kI = arm_kI.get();
+        IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get();
-        IntakeConstants.ARM_PID.kD = arm_kD.get();
+        IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
         m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
 
-        IntakeConstants.ROLLER_PID.kP = roller_kP.get();
+        IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get();
-        IntakeConstants.ROLLER_PID.kI = roller_kI.get();
+        IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get();
-        IntakeConstants.ROLLER_PID.kD = roller_kD.get();
+        IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get();
         m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
     }
 }

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -1,18 +1,11 @@
 package frc4388.robot.subsystems.shooter;
 
-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 java.util.function.Supplier;
-
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.subsystems.intake.IntakeConstants;
-import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState;
 
 public class Shooter extends SubsystemBase {
     public ShooterIO io;
@@ -47,6 +40,7 @@ public class Shooter extends SubsystemBase {
         Inactive,
     }
 
+
     public void setMode(ShooterMode mode) {
         switch (mode) {
             case Active:
@@ -57,12 +51,12 @@ public class Shooter extends SubsystemBase {
             case Resting:
                 io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
                 // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
-                io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get()));
+                io.setIndexerVelocity(state, RotationsPerSecond.of(0));
                 break;
             case Inactive:
                 io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
                 // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
-                io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get()));
+                io.setIndexerVelocity(state, RotationsPerSecond.of(0));
                 break;
         }
     }

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -31,10 +31,10 @@ public class ShooterConstants {
 
     public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
     public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
-    public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
+    // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
     
     public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10);
-    public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
+    // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
 
     public static Slot0Configs SHOOTER_PID = new Slot0Configs()
         .withKV(0.0)
@@ -48,6 +48,15 @@ public class ShooterConstants {
         .withKI(0.0)
         .withKD(0.0);
     
+    
+    public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
+    public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0);
+    public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0);
+    
+    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2);
+    public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0);
+    public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
+
     // Limits
 
     // 0 is the forward angle on the robot.

src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java

@@ -94,6 +94,12 @@ public class ShooterReal implements ShooterIO {
         state.motor1TargetVelocity = target;
         state.motor2TargetVelocity = target;
 
+        if(target.baseUnitMagnitude() == 0) {
+            m_shooter1Motor.set(0);
+            m_shooter2Motor.set(0);
+            return;
+        }
+
         AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
 
         m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps));
@@ -108,14 +114,6 @@ public class ShooterReal implements ShooterIO {
     }
 
 
-    ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
-    ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0);
-    ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0);
-    
-    ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2);
-    ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0);
-    ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
-
     @Override
     public void updateInputs(ShooterState state) {
 
@@ -134,15 +132,15 @@ public class ShooterReal implements ShooterIO {
     @Override 
     public void updateGains() {
         // TEMPORARY PIDs
-        ShooterConstants.SHOOTER_PID.kP = shooter_kP.get();
+        ShooterConstants.SHOOTER_PID.kP = ShooterConstants.shooter_kP.get();
-        ShooterConstants.SHOOTER_PID.kI = shooter_kI.get();
+        ShooterConstants.SHOOTER_PID.kI = ShooterConstants.shooter_kI.get();
-        ShooterConstants.SHOOTER_PID.kD = shooter_kD.get();
+        ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get();
         m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
         m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
 
-        ShooterConstants.INDEXER_PID.kP = indexer_kP.get();
+        ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get();
-        ShooterConstants.INDEXER_PID.kI = indexer_kI.get();
+        ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get();
-        ShooterConstants.INDEXER_PID.kD = indexer_kD.get();
+        ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get();
         m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
     }