intake calibration

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
index c472bda9704dc76405659b1d65dd2c50d631c0de..0cba508b784fb364a68ab07796164110384ac02a 100644 (file)
--- 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 e48786a82c7d7743398c9df1e929ee01feae473e..00278b12f918992ac42da02ca0afc8d0518efe70 100644 (file)
--- 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 f91063f5e750152a52a0d6a4c8ab5413109ff762..7600657db4a9e5d6aeeedacdd2b0f1da546a04da 100644 (file)
--- 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());