Soft limits & Homing Command

Author: Orcasphynx

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

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.intake;
 
 import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 
 public class IntakeConstants {
 
@@ -29,6 +30,8 @@ public static Slot0Configs createRollerMotorSlot0Configs() {
 
   // Extension Motor
   public static final double ALLOWABLE_EXTENSION_ERROR = 0.1;
+  public static final double INTAKE_FORWARD_LIMIT = 100;
+  public static final double INTAKE_REVERSE_LIMIT = 0;
   public static final double EXTENSION_KS = 0;
   public static final double EXTENSION_KP = 0;
   public static final double EXTENSION_KD = 0;
@@ -40,4 +43,16 @@ public static Slot0Configs createExtensionMotorSlot0Configs() {
     slot.kD = EXTENSION_KD;
     return slot;
   }
+
+  public static SoftwareLimitSwitchConfigs createExtensionSoftwareLimitSwitchConfigs() {
+    SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs();
+    configs.ForwardSoftLimitEnable = false;
+    configs.ReverseSoftLimitEnable = false;
+    configs.ForwardSoftLimitThreshold = INTAKE_FORWARD_LIMIT;
+    configs.ReverseSoftLimitThreshold = INTAKE_REVERSE_LIMIT;
+    return configs;
+  }
+
+  public static final double SAFE_HOMING_EFFORT = -0.2;
+  public static final double SAFE_STATOR_LIMIT = 0.8;
 }

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

@@ -31,9 +31,14 @@ public IntakeSubsystem() {
     rollerControl = new VelocityVoltage(0);
 
     extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorSlot0Configs());
+    extensionMotor
+        .getConfigurator()
+        .apply(IntakeConstants.createExtensionSoftwareLimitSwitchConfigs());
     extensionMotor.setPosition(0);
     extensionTarget = Rotations.of(0);
     extensionControl = new PositionTorqueCurrentFOC(0);
+
+    homeIntake();
   }
 
   @Override
@@ -77,4 +82,23 @@ public Command stowCommand() {
         .andThen(setIntakeExtensionCommand(Rotations.of(0)))
         .withName("Stow Intake");
   }
+
+  private void homeIntake() {
+    while (extensionMotor.getStatorCurrent().getValueAsDouble()
+        > IntakeConstants.SAFE_STATOR_LIMIT) {
+      extensionMotor.set(IntakeConstants.SAFE_HOMING_EFFORT);
+    }
+    extensionMotor.setPosition(0);
+  }
+
+  public Command homeIntakeCommand() {
+    return runEnd(
+            () -> extensionMotor.set(IntakeConstants.SAFE_HOMING_EFFORT),
+            () -> extensionMotor.setPosition(0))
+        .until(
+            () -> {
+              return extensionMotor.getStatorCurrent().getValueAsDouble()
+                  > IntakeConstants.SAFE_STATOR_LIMIT;
+            });
+  }
 }