what autoscoring would look like with the limit switch CTRE "hack" and a fictitious canrange

Author: karlolson5

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -22,7 +22,6 @@
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.vision.VisionFieldPoseEstimate;
 import frc.robot.util.RobotTime;
-
 import frc.robot.util.simulations.MapleSimSwerveDrivetrain;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;

src/main/java/frc/robot/subsystems/end_effector/Claw.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.EndEffectorConstants;
@@ -39,6 +40,7 @@ public class Claw extends SubsystemBase {
   private ClawState currentState = ClawState.NONE;
   private boolean firstSensorTriggered;
   private boolean secondSensorTriggered;
+  private boolean autoScoring = false;
 
   public Claw(ClawIO io) {
     this.io = io;
@@ -83,6 +85,13 @@ public boolean isClawScoring() {
         || currentState == ClawState.SCORING_ALGAE;
   }
 
+  public Command autoScoreWhenPoleSensed() {
+    return new InstantCommand(() -> io.setRollerPosition(0), this)
+        .alongWith(new InstantCommand(() -> io.setRollerTargetPosition(0)))
+        .until(
+            () -> CoralStateTracker.getCurrentPosition() == CoralStateTracker.CoralPosition.NONE);
+  }
+
   public boolean isCoralInClaw() {
     // Use CoralStateTracker instead of individual sensor readings
     CoralStateTracker.CoralPosition position = CoralStateTracker.getCurrentPosition();

src/main/java/frc/robot/subsystems/end_effector/ClawIO.java

@@ -9,6 +9,7 @@ class ClawIOInputs {
     public EE_RollerData rollerData = new EE_RollerData(false, 0, 0, 0, 0, 0);
     public EE_CANRangeData firstCANRangeData = new EE_CANRangeData(false, false);
     public EE_CANRangeData secondCANRangeData = new EE_CANRangeData(false, false);
+    public EE_CANRangeData scoringCANRangeData = new EE_CANRangeData(false, false);
   }
 
   record EE_RollerData(
@@ -29,6 +30,10 @@ default void setTorqueCurrent(double amps) {}
 
   default void setRollerVelocity(double velocity) {}
 
+  default void setRollerPosition(double position) {}
+
+  default void setRollerTargetPosition(double position) {}
+
   default boolean checkRollerStalled() {
     return false;
   }

src/main/java/frc/robot/subsystems/end_effector/ClawIOReal.java

@@ -2,6 +2,9 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.CANrange;
@@ -22,6 +25,7 @@ public class ClawIOReal implements ClawIO {
   protected TalonFX rollerTalonFX;
   private CANrange firstCoralCANRange;
   private CANrange secondCoralCANRange;
+  private CANrange scoringCANRange;
 
   private TorqueCurrentFOC roller_c_request =
       new TorqueCurrentFOC(EndEffectorConstants.CLAW_HOLD_ALGAE_AMPS);
@@ -46,12 +50,26 @@ public ClawIOReal() {
         new CANrange(EndEffectorConstants.FIRST_CORAL_CANRANGE_ID, Constants.MISC_CANIVORE);
     secondCoralCANRange =
         new CANrange(EndEffectorConstants.SECOND_CORAL_CANRANGE_ID, Constants.MISC_CANIVORE);
+    scoringCANRange = new CANrange(34, Constants.MISC_CANIVORE);
     PhoenixUtil.tryUntilOk(
         5, () -> firstCoralCANRange.getConfigurator().apply(EndEffectorConstants.CANRANGE_CONFIG));
     PhoenixUtil.tryUntilOk(
         5, () -> secondCoralCANRange.getConfigurator().apply(EndEffectorConstants.CANRANGE_CONFIG));
     PhoenixUtil.tryUntilOk(
-        5, () -> rollerTalonFX.getConfigurator().apply(EndEffectorConstants.ROLLER_TALON_CONFIG));
+        5, () -> scoringCANRange.getConfigurator().apply(EndEffectorConstants.CANRANGE_CONFIG));
+    PhoenixUtil.tryUntilOk(
+        5,
+        () ->
+            rollerTalonFX
+                .getConfigurator()
+                .apply(
+                    EndEffectorConstants.ROLLER_TALON_CONFIG
+                        .withHardwareLimitSwitch(
+                            new HardwareLimitSwitchConfigs()
+                                .withReverseLimitAutosetPositionEnable(true)
+                                .withReverseLimitAutosetPositionValue(-100)
+                                .withReverseLimitRemoteCANrange(scoringCANRange))
+                        .withSlot0(new Slot0Configs().withKP(100))));
 
     rollerVelocityRPS = rollerTalonFX.getRotorVelocity();
     rollerAppliedVolts = rollerTalonFX.getMotorVoltage();
@@ -120,6 +138,16 @@ public void setRollerVoltage(double voltage) {
     rollerTalonFX.setControl(roller_m_request.withOutput(voltage));
   }
 
+  @Override
+  public void setRollerPosition(double position) {
+    rollerTalonFX.setPosition(0);
+  }
+
+  @Override
+  public void setRollerTargetPosition(double position) {
+    rollerTalonFX.setControl(new PositionVoltage(position));
+  }
+
   @Override
   public boolean checkRollerStalled() {
     return debouncerAlgae.calculate(