From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 21 Feb 2026 22:23:21 +0000 (-0800) Subject: shud work X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=5ee73d3da0f228a821ccc7297bff61a2d1c28d6a;p=FRC2026.git shud work --- diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 5a7720a..578b8cb 100644 --- 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 bc25a2d..e916934 100644 --- 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 dc48183..ac48c40 100644 --- 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 3491482..2b14eba 100644 --- 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 fbb59de..43a3eaf 100644 --- 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 e1b75c8..cc20e67 100644 --- 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 aa6e5d6..b1079ff 100644 --- 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 }