shud work

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
index 5a7720ae1ad5f2015d0b4f8d71e787602794a0ea..578b8cbfcef99cff755cbf3d57cd7cd0e809dbc7 100644 (file)
--- a/src/main/java/frc/robot/constants/IntakeConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -12,7 +12,8 @@ public class IntakeConstants {
     /**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 CALIBRATING_CURRENT_LIMITS = 10.0;
+    public static final double CALIBRATING_CURRENT_THRESHOLD = 9.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 bc25a2d16636f65539bfc2ebb169cba3fb39bf8b..e9169348f592575b034c0e699ac39095aa3a6081 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -109,7 +109,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             }));
 
             // Calibration
-            driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{
+            driver.get(PS5Button.PS).onTrue(new InstantCommand(()->{
                 intake.calibrate();
             })).onFalse(new InstantCommand(()->{
                 intake.stopCalibrating();

diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
index dc48183212a58f1710e14a4cc196df281a5dd020..ac48c406294009cf6d44e62118ce005101cbe4b6 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
@@ -21,6 +21,7 @@ public class ClimbConstants {
     // CLIMB: High current for full-power climbing
     public final static double CALIBRATION_CURRENT = 7.0;
     public final static double CLIMB_CURRENT = 42.0;
+    public final static double CALIBRATION_CURRENT_THRESHOLD = 6.0;
 
     // PID Constants
     // TODO: what are the units? Inches? Meters?

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 34914828b9bbb213e73b9f401dff5a9c8614f078..2b14eba9881d42d6b68215a0b03099c031aaa246 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -10,6 +10,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -26,20 +28,16 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
     /** whether the subsysgtem is calibrating */
     private boolean calibrating = false;
 
-    // why not use the ClimbConstants directly?
-    private double downPosition = ClimbConstants.BOTTOM_POSITION;
-    private double upPosition = ClimbConstants.UP_POSITION;
-    private double climbPosition = ClimbConstants.CLIMB_POSITION;
-
     /** should the subsystem perform calibration automatically */
     private boolean calibrateOnStartUp = false;
 
     private double MAX_POWER = 0.2;
 
+    private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
+
     // logging information
     private LinearClimbIOInputs inputs = new LinearClimbIOInputs();
 
-    // TODO: what units?
     private final PIDController pid = new PIDController(
             ClimbConstants.PID_P,
             ClimbConstants.PID_I,
@@ -139,6 +137,10 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
             power = MathUtil.clamp(power, -MAX_POWER, MAX_POWER);
         } else{
             power = ClimbConstants.CALIBRATION_POWER;
+            boolean atHardStop = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= ClimbConstants.CALIBRATION_CURRENT_THRESHOLD;
+            if(calibrationDebouncer.calculate(atHardStop)){
+                stopCalibrating();
+            }
         }
         motor.set(power);
         
@@ -195,10 +197,10 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      * stops calibration and sets current limits to normal. 
      */
     public void stopCalibrating() {
-        motor.setPosition((Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
+        motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION));
         calibrating = false;
         setCurrentLimits(ClimbConstants.CLIMB_CURRENT);
-        setSetpoint((Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
+        setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index fbb59de347cfd27f93afd70ec50d4d13fa233e95..43a3eafaf97828a596b7a5ad7aab6d2edd24bd54 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -11,6 +11,8 @@ import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
@@ -64,6 +66,7 @@ public class Intake extends SubsystemBase implements IntakeIO{
     private double setpointInches = 0.0;
 
     private boolean calibrating = false;
+    private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
 
     private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
 
@@ -183,6 +186,10 @@ public class Intake extends SubsystemBase implements IntakeIO{
         if(calibrating){
             leftMotor.set(0.1);
             rightMotor.set(-0.1);
+            boolean atHardStop = Math.abs((leftMotor.getStatorCurrent().getValueAsDouble() + rightMotor.getStatorCurrent().getValueAsDouble()) / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD;
+            if(calibrationDebouncer.calculate(atHardStop)){
+                stopCalibrating();
+            }
         }
 
         updateInputs();
@@ -341,8 +348,6 @@ public class Intake extends SubsystemBase implements IntakeIO{
      */
     public void stopCalibrating(){
         zeroMotors();
-        leftMotor.set(0);
-        rightMotor.set(0);
         setCurrentLimits(IntakeConstants.EXTENDER_CURRENT_LIMITS);
         calibrating = false;
         retract();

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index e1b75c8a9ee1f79d3d4d3d87e11eb937a5a968d9..cc20e67f9eef473fd9948b09597d1d18b91fa2be 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -118,6 +118,11 @@ public class Hood extends SubsystemBase implements HoodIO{
 
                if (calibrating){
                        motor.set(0.1);
+                       boolean atZero = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD;
+                       boolean calibrated = calibrateDebouncer.calculate(atZero);
+                       if (calibrated){
+                               stopCalibrating();
+                       }
                } else{
                        // Set control with feedforward
                        motor.setControl(mmVoltageRequest
@@ -134,13 +139,13 @@ public class Hood extends SubsystemBase implements HoodIO{
        public void calibrate(){
                calibrating = true;
                setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT);
-               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);
-                       setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
-               }
+       }
+
+       public void stopCalibrating(){
+               motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
+               goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
+               setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
+               calibrating = false;
        }
 
        /**

diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
index aa6e5d620d6aebaa2c494d22216107ca312f3024..b1079ffce01f6fe1335a329e8fb92324dfc266b3 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
@@ -18,6 +18,6 @@ public class HoodConstants {
     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
-    public static final double CALIBRATION_CURRENT_THRESHOLD = 20.0; // A
+    public static final double CALIBRATING_CURRENT_LIMIT = 10.0; //A
+    public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
 }