From 057e8c3ecb01d9e7534fa17efadf4c2330e4b073 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Sat, 21 Feb 2026 11:54:02 -0800
Subject: [PATCH] m

---
 .../java/frc/robot/subsystems/hood/Hood.java  | 32 +++++++++++++++++--
 .../robot/subsystems/hood/HoodConstants.java  |  3 ++
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 34f8c27..b44009e 100644
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -2,6 +2,7 @@ package frc.robot.subsystems.hood;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -9,7 +10,9 @@ import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -31,6 +34,7 @@ public class Hood extends SubsystemBase implements HoodIO{
 	private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
 
 	private boolean calibrating = false;
+	private Debouncer calibrateDebouncer = new Debouncer(0.5, DebounceType.kRising);
 
     private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
 
@@ -51,6 +55,8 @@ public class Hood extends SubsystemBase implements HoodIO{
 		mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
         motor.getConfigurator().apply(config);
 
+		setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
+
 		motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
 
 		SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
@@ -110,10 +116,14 @@ public class Hood extends SubsystemBase implements HoodIO{
 		// Multiply goal velocity by kV
 		double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV;
 
-		// Set control with feedforward
-		motor.setControl(mmVoltageRequest
+		if (calibrating){
+			motor.set(0.1);
+		} else{
+			// Set control with feedforward
+			motor.setControl(mmVoltageRequest
 			.withPosition(motorGoalRotations)
 			.withFeedForward(velocityCompensation));
+		}
 
         Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
 		Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO);
@@ -122,9 +132,25 @@ public class Hood extends SubsystemBase implements HoodIO{
 	}
 
 	public void calibrate(){
-
+		calibrating = true;
+		setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT);
+		calibrateDebouncer.calculate(calibrating)
 	}
 
+	/**
+     * sets supply and stator current limits
+     * @param limitAmps the current limit for stator and supply current
+     */
+    public void setCurrentLimits(double limitAmps) {
+        CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
+        .withStatorCurrentLimitEnable(true)
+        .withStatorCurrentLimit(limitAmps)
+        .withSupplyCurrentLimitEnable(true)
+        .withSupplyCurrentLimit(limitAmps);
+
+        motor.getConfigurator().apply(limits);
+    }
+
     @Override
 	public void updateInputs() {
 		inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
index e38e680..3c809cb 100644
--- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
@@ -16,4 +16,7 @@ public class HoodConstants {
     public static final double MIN_ANGLE = 58.5; // degrees 
 
     public static final double FEEDFORWARD_KV = 0.12;
+
+    public static final double NORMAL_CURRENT_LIMIT = 40.0; // A
+    public static final double CALIBRATING_CURRENT_LIMIT = 30.0; //A
 }
-- 
2.39.5