a

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index b44009ea85b9f599efc97cbe519240e9078ee95a..d8b70872143113b988fe28aee1e6df23d134cab8 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -134,7 +134,12 @@ public class Hood extends SubsystemBase implements HoodIO{
        public void calibrate(){
                calibrating = true;
                setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT);
-               calibrateDebouncer.calculate(calibrating)
+               boolean atZero = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD;
+               boolean calibrated = calibrateDebouncer.calculate(atZero);
+               if (calibrated){
+                       calibrating = false;
+                       motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * 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 3c809cb6ac2204541b86c040e75e6bb110ed686f..aa6e5d620d6aebaa2c494d22216107ca312f3024 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
@@ -19,4 +19,5 @@ public class HoodConstants {
 
     public static final double NORMAL_CURRENT_LIMIT = 40.0; // A
     public static final double CALIBRATING_CURRENT_LIMIT = 30.0; //A
+    public static final double CALIBRATION_CURRENT_THRESHOLD = 20.0; // A
 }