From d510b78ffcd597b14808716b4c8fc06306915b42 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Fri, 20 Feb 2026 14:58:52 -0800
Subject: [PATCH] intake calibration

---
 .../frc/robot/constants/IntakeConstants.java  |  2 ++
 .../controls/PS5ControllerDriverConfig.java   |  7 +++++
 .../frc/robot/subsystems/Intake/Intake.java   | 28 +++++++++++++++++++
 3 files changed, 37 insertions(+)

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
index c472bda..0cba508 100644
--- a/src/main/java/frc/robot/constants/IntakeConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -11,6 +11,8 @@ public class IntakeConstants {
     public static final double RADIUS_RACK_PINION = 0.5;
     /**right and left motor current limits */
     public static final double EXTENDER_CURRENT_LIMITS = 40.0;
+    /**Current limits when calibrating */
+    public static final double CALIBRATING_CURRENT_LIMITS = 30.0;
 
     public static final double ROLLER_MOI_KG_M_SQ = 0.5 * 0.020 * 0.020; // 0.5kg roller, 20mm radius for now
     public static final double ROLLER_GEARING = 2.0;
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index e48786a..00278b1 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -93,6 +93,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 intake.retract();
                 intakeBoolean = true;
             }));
+
+            // Calibration
+            driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{
+                intake.calibrate();
+            })).onFalse(new InstantCommand(()->{
+                intake.stopCalibrating();
+            }));
         }
 
         // Spindexer
diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index f91063f..7600657 100644
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -2,6 +2,7 @@ package frc.robot.subsystems.Intake;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -342,15 +343,42 @@ public class Intake extends SubsystemBase implements IntakeIO{
         rollerMotor.close();
     }
 
+    /**
+     * Starts calibrating by running it backwards
+     */
     public void calibrate(){
+        setCurrentLimits(IntakeConstants.CALIBRATING_CURRENT_LIMITS);
         calibrating = true;
     }
+
+    /**
+     * Stops, zeros, and moves it to retract position
+     */
     public void stopCalibrating(){
         calibrating = false;
         zeroMotors();
+        setCurrentLimits(IntakeConstants.EXTENDER_CURRENT_LIMITS);
         retract();
     }
 
+    /**
+     * sets supply and stator current limits
+     * @param limit the current limit for stator and supply current
+     */
+    public void setCurrentLimits(double limit) {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+
+        config.CurrentLimits = new CurrentLimitsConfigs();
+
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+        config.CurrentLimits.StatorCurrentLimit = limit;
+        config.CurrentLimits.SupplyCurrentLimitEnable = true;
+        config.CurrentLimits.SupplyCurrentLimit = limit;
+
+        leftMotor.getConfigurator().apply(config);
+        rightMotor.getConfigurator().apply(config);
+    }
+
     @Override
     public void updateInputs() {
         inputs.leftPosition = rotationsToInches(leftMotor.getPosition().getValueAsDouble());
-- 
2.39.5