yay

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 8b358ad18974aee08bddea5eedca8e952b703eef..ae6e7a86a652ab760f77182789ae284e949bd827 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -6,8 +6,6 @@ public class ClimbConstants {
     public final static double CLIMB_GEAR_RATIO = 9.0 / 1 * 5.0 / 1;
     public final static double MAX_VELOCITY = 1;
     public final static double MAX_ACCELERATION = 0.3;
-    public final static double MIN_HEIGHT = 0;
-    public final static double MAX_HEIGHT = 8;
     public final static double RADIUS = 0.3;
     public final static double CLIMB_HEIGHT = 4;
 }

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index e27fb871f41d71784cd7489056d65341aae9a8a5..b3bcc6b6e7422d1d26fe2183f09a4f411dd4e723 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -25,6 +25,7 @@ import frc.robot.constants.swerve.DriveConstants;
 
 public class LinearClimb extends SubsystemBase{
     private TalonFX motor;
+    private boolean calibrating = false;
 
     private static PIDController pid = new PIDController(0.1, 0, 0);
     
@@ -36,20 +37,12 @@ public class LinearClimb extends SubsystemBase{
 
         motor.setNeutralMode(NeutralModeValue.Brake);
 
-        TalonFXConfiguration config = new TalonFXConfiguration();
-
-        config.CurrentLimits = new CurrentLimitsConfigs();
-
-        config.CurrentLimits.StatorCurrentLimitEnable = true;
-        config.CurrentLimits.StatorCurrentLimit = 5.0;
-        config.CurrentLimits.SupplyCurrentLimitEnable = true;
-        config.CurrentLimits.SupplyCurrentLimit = 5.0;
+        setCurrentLimits(5.0);
 
-        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-        motor.getConfigurator().apply(config);
         SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
         SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown()));
         SmartDashboard.putData("Climb", new InstantCommand(() -> climb()));
+        SmartDashboard.putData("Hardstop Calibrate", new InstantCommand(() -> hardstopCalibration()));
         SmartDashboard.putNumber("Position", getPosition());
 
         motor.setPosition(0);
@@ -72,11 +65,11 @@ public class LinearClimb extends SubsystemBase{
     }
 
     public void goUp() {
-        setSetpoint(45);
+        setSetpoint(0);
     }
 
     public void goDown() {
-        setSetpoint(ClimbConstants.MIN_HEIGHT);
+        setSetpoint(0);
     }
 
     public void climb() {
@@ -84,9 +77,30 @@ public class LinearClimb extends SubsystemBase{
     }
 
     public void periodic() {
-        double power = pid.calculate(motor.getPosition().getValueAsDouble());
-        power = MathUtil.clamp(power, -0.2, 0.2);
-        motor.set(power);
-        SmartDashboard.putNumber("Position", getPosition());
+        if(calibrating == false){
+            double power = pid.calculate(motor.getPosition().getValueAsDouble());
+            power = MathUtil.clamp(power, -0.2, 0.2);
+            motor.set(power);
+            SmartDashboard.putNumber("Position", getPosition());
+        }else{
+            motor.set(0.15);
+        }
+        
+    }
+    public void setCurrentLimits(double limit){
+        TalonFXConfiguration config = new TalonFXConfiguration();
+
+        config.CurrentLimits = new CurrentLimitsConfigs();
+
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+        config.CurrentLimits.StatorCurrentLimit = limit;
+        config.CurrentLimits.SupplyCurrentLimitEnable = true;
+        config.CurrentLimits.SupplyCurrentLimit = limit;
+
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        motor.getConfigurator().apply(config);
+    }
+    public void hardstopCalibration(){
+        calibrating = !calibrating;
     }
 }
\ No newline at end of file