Feature/pid refactoring

src/main/java/frc/robot/RobotContainer.java

@@ -186,8 +186,7 @@ public void configureTestBindings() {
 
     driverJoystick
         .a()
-        .whileTrue(driveAndLaunchCommand.alongWith(indexer.startFullIndexingNoPID()))
-        .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood()));
+        .whileTrue(indexer.startFullIndexingNoPID());
 
     driverJoystick.b().onTrue(intake.testRollerNoPID()).onFalse(intake.stopRollerNoPID());
 

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

@@ -1,7 +1,7 @@
 package frc.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
@@ -70,10 +70,10 @@ public static OpenLoopRampsConfigs createRollerMotorRampConfigs() {
   // Extension Motor
   public static final double ALLOWABLE_EXTENSION_ERROR = 1;
 
-  public static final double EXTENSION_KS = 0.468;
-  public static final double EXTENSION_KP = 7.5;
-  public static final double EXTENSION_KI = 1.75;
-  public static final double EXTENSION_KD = 0.3;
+  public static final double EXTENSION_KS = 0.47;
+  public static final double EXTENSION_KP = 5.0;
+  public static final double EXTENSION_KI = 0.0;
+  public static final double EXTENSION_KD = 0.5;
 
   public static Slot0Configs createExtensionMotorSlot0Configs() {
     Slot0Configs slot = new Slot0Configs();
@@ -125,24 +125,23 @@ public static MotorOutputConfigs createExtensionMotorOutputConfigs() {
     return newConfigs;
   }
 
-  public static final double EXTENSION_MM_CRUISE_VELOCITY = 40;
-  public static final double EXTENSION_MM_ACCELERATION = 60;
-  public static final double EXTENSION_MM_JERK = 400;
-
-  public static MotionMagicConfigs createExtenstionMotionMagicConfigs() {
-    MotionMagicConfigs newConfigs = new MotionMagicConfigs();
-    newConfigs.MotionMagicCruiseVelocity = 40;
-    newConfigs.MotionMagicAcceleration = 60;
-    newConfigs.MotionMagicJerk = 400;
-    return newConfigs;
-  }
-
-  public static final double EXTENSION_CURRENT_LIMIT = 40;
+  public static final double EXTENSION_STATOR_CURRENT_LIMIT = 75;
+  public static final double EXTENSION_SUPPLY_CURRENT_LIMIT = 40;
 
   public static CurrentLimitsConfigs createExtenstionMotorCurrentLimitsConfigs() {
     CurrentLimitsConfigs config = new CurrentLimitsConfigs();
     config.StatorCurrentLimitEnable = true;
-    config.StatorCurrentLimit = EXTENSION_CURRENT_LIMIT;
+    config.StatorCurrentLimit = EXTENSION_STATOR_CURRENT_LIMIT;
+    config.SupplyCurrentLimitEnable = true;
+    config.SupplyCurrentLimit = EXTENSION_SUPPLY_CURRENT_LIMIT;
+    return config;
+  }
+
+  public static final double VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5;
+
+  public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs() {
+    ClosedLoopRampsConfigs config = new ClosedLoopRampsConfigs();
+    config.VoltageClosedLoopRampPeriod = VOLTAGE_CLOSED_LOOP_RAMP_PERIOD;
     return config;
   }
 

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

@@ -6,8 +6,7 @@
 
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
-import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
@@ -36,9 +35,7 @@ public class IntakeSubsystem extends SubsystemBase {
   @Logged(name = "Extension Target", importance = Importance.CRITICAL)
   private Angle extensionTarget = Rotations.of(IntakeConstants.INTAKE_REVERSE_LIMIT); // Rotations
 
-  private PositionTorqueCurrentFOC extensionControl;
-
-  private MotionMagicTorqueCurrentFOC mmExtenstionControl;
+  private PositionVoltage extensionControl;
 
   @Logged(name = "Extension Is Compliant", importance = Importance.CRITICAL)
   private boolean isCompliantMode;
@@ -81,15 +78,14 @@ public IntakeSubsystem() {
         .apply(IntakeConstants.createExtensionSoftwareLimitSwitchConfigs());
     extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorOutputConfigs());
     extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorSlot1Configs());
-    extensionMotor.getConfigurator().apply(IntakeConstants.createExtenstionMotionMagicConfigs());
+    extensionMotor.getConfigurator().apply(IntakeConstants.creatClosedLoopRampsConfigs());
     extensionMotor
         .getConfigurator()
         .apply(IntakeConstants.createExtenstionMotorCurrentLimitsConfigs());
 
     extensionMotor.setPosition(0);
     extensionTarget = Rotations.of(0);
-    extensionControl = new PositionTorqueCurrentFOC(0);
-    mmExtenstionControl = new MotionMagicTorqueCurrentFOC(0);
+    extensionControl = new PositionVoltage(0);
     isCompliantMode = false;
   }
 
@@ -109,8 +105,7 @@ public void periodic() {
     extensionMotor.setControl(
         extensionControl
             .withSlot(isCompliantMode ? 1 : 0)
-            .withPosition(extensionTarget.in(Rotations))
-            .withOverrideCoastDurNeutral(true));
+            .withPosition(extensionTarget.in(Rotations)));
   }
 
   public Command spinRollerCommand() {

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

@@ -2,8 +2,8 @@
 
 import static edu.wpi.first.units.Units.*;
 
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
@@ -63,41 +63,34 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs()
     return configs;
   }
 
-  public static final double HOOD_MAX_VELOCITY_RPS = 5;
-  public static final double HOOD_MAX_ACCELERATION_RPS2 = 10;
-  public static final double HOOD_MAX_JERK_RPS2 = 10;
-
-  public static MotionMagicConfigs createHoodMotionMagicConfigs() {
-    MotionMagicConfigs configs = new MotionMagicConfigs();
-    configs.MotionMagicCruiseVelocity = HOOD_MAX_VELOCITY_RPS;
-    configs.MotionMagicAcceleration = HOOD_MAX_ACCELERATION_RPS2;
-    configs.MotionMagicJerk = HOOD_MAX_JERK_RPS2;
-
-    return configs;
-  }
-
-  public static final double FLYWHEEL_CURRENT_LIMIT = 40;
+  public static final double FLYWHEEL_STATOR_CURRENT_LIMIT = 100;
+  public static final double FLYWHEEL_SUPPLY_CURRENT_LIMIT = 40;
 
   public static CurrentLimitsConfigs createFlywheelCurrentLimitsConfigs() {
     CurrentLimitsConfigs configs = new CurrentLimitsConfigs();
-    configs.StatorCurrentLimit = FLYWHEEL_CURRENT_LIMIT;
+    configs.StatorCurrentLimit = FLYWHEEL_STATOR_CURRENT_LIMIT;
+    configs.SupplyCurrentLimit = FLYWHEEL_SUPPLY_CURRENT_LIMIT;
     configs.StatorCurrentLimitEnable = true;
+    configs.SupplyCurrentLimitEnable = true;
     return configs;
   }
 
-  public static final double HOOD_CURRENT_LIMIT = 40;
+  public static final double HOOD_STATOR_CURRENT_LIMIT = 50;
+  public static final double HOOD_SUPPLY_CURRENT_LIMIT = 40;
 
   public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() {
     CurrentLimitsConfigs configs = new CurrentLimitsConfigs();
-    configs.StatorCurrentLimit = HOOD_CURRENT_LIMIT;
+    configs.StatorCurrentLimit = HOOD_STATOR_CURRENT_LIMIT;
+    configs.SupplyCurrentLimit = HOOD_SUPPLY_CURRENT_LIMIT;
     configs.StatorCurrentLimitEnable = true;
+    configs.SupplyCurrentLimitEnable = true;
     return configs;
   }
 
-  public static final double HOOD_KS = 4.8;
-  public static final double HOOD_KP = 9;
-  public static final double HOOD_KD = 0.1;
-  public static final double HOOD_KI = 1;
+  public static final double HOOD_KS = 0.47;
+  public static final double HOOD_KP = 5.5;
+  public static final double HOOD_KD = 0.266;
+  public static final double HOOD_KI = 0.0;
 
   public static Slot0Configs createHoodMotorSlot0Configs() {
     Slot0Configs slot = new Slot0Configs();
@@ -108,6 +101,13 @@ public static Slot0Configs createHoodMotorSlot0Configs() {
     return slot;
   }
 
+  public static final double HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5;
+
+  public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs() {
+    return new ClosedLoopRampsConfigs()
+        .withVoltageClosedLoopRampPeriod(HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD);
+  }
+
   // TODO: Tune Flywheel and Hood Motor
 
   // Flywheel motor

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

@@ -34,6 +34,10 @@ public class ShooterSubsystem extends SubsystemBase {
   @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL)
   private Angle hoodTarget; // radians is the base unit.
 
+  // For testing
+  private double lastRPS = 0;
+  private double lastHoodRot = 0;
+
   private PositionVoltage hoodControl;
 
   final SysIdRoutine m_sysIdRoutineFlywheel =
@@ -77,7 +81,7 @@ public ShooterSubsystem() {
 
     hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs());
 
-    hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotionMagicConfigs());
+    hoodMotor.getConfigurator().apply(ShooterConstants.creatClosedLoopRampsConfigs());
 
     hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs());
 
@@ -129,6 +133,11 @@ public double getFlywheelTargetRPM() {
     return velocityTarget.in(RotationsPerSecond) * 60;
   }
 
+  @Logged(name = "Velocity Target RPS", importance = Importance.CRITICAL)
+  public double getFlywheelTargetRPMS() {
+    return velocityTarget.in(RotationsPerSecond);
+  }
+
   public Command spinFlywheelCommand() {
     return runOnce(
             () -> {
@@ -140,12 +149,20 @@ public Command spinFlywheelCommand() {
   }
 
   public Command stopFlywheelCommand() {
-    return runOnce(() -> velocityTarget = RotationsPerSecond.of(0))
+    return runOnce(
+            () -> {
+              lastRPS = velocityTarget.in(RotationsPerSecond);
+              velocityTarget = RotationsPerSecond.of(0);
+            })
         .withName("Stop Spinning Flywheel");
   }
 
   public Command stowHood() {
-    return runOnce(() -> hoodTarget = Rotations.of(0));
+    return runOnce(
+        () -> {
+          lastHoodRot = hoodTarget.in(Rotations);
+          hoodTarget = Rotations.of(0);
+        });
   }
 
   public Command increaseFlywheelCommand() {
@@ -189,11 +206,17 @@ public Command decreaseHoodCommand() {
   }
 
   public Command startShooterTuningCommand() {
+
     return runOnce(
             () -> {
-              velocityTarget =
-                  RotationsPerSecond.of(ShooterPreferences.tuningDefaultFlywheelRPS.getValue());
-              hoodTarget = Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue());
+              double flywheelRPS =
+                  lastRPS > 0 ? lastRPS : ShooterPreferences.tuningDefaultFlywheelRPS.getValue();
+              double hoodRot =
+                  lastHoodRot > 0
+                      ? lastHoodRot
+                      : ShooterPreferences.tuningDefaultHoodRotations.getValue();
+              velocityTarget = RotationsPerSecond.of(flywheelRPS);
+              hoodTarget = Rotations.of(hoodRot);
             })
         .withName("Start Shooter Tuning");
   }