m

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 34f8c27aa75817f93e4d8ba1697df4162abe7f9d..b44009ea85b9f599efc97cbe519240e9078ee95a 100644 (file)
--- 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 e38e6805982e40caa4216ab3267adbd2b4173cab..3c809cb6ac2204541b86c040e75e6bb110ed686f 100644 (file)
--- 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
 }