From a55a3101ffc7a51bfef0725630c6a612badbd13b Mon Sep 17 00:00:00 2001
From: Ethan Mortensen <ethanmortensen20@gmail.com>
Date: Sat, 14 Feb 2026 14:51:51 -0800
Subject: [PATCH] calibration

---
 .../robot/constants/Climb/ClimbConstants.java |  4 +++
 .../robot/subsystems/Climb/LinearClimb.java   | 29 +++++++++++++++----
 2 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index ae6e7a8..328b1c7 100644
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -8,4 +8,8 @@ public class ClimbConstants {
     public final static double MAX_ACCELERATION = 0.3;
     public final static double RADIUS = 0.3;
     public final static double CLIMB_HEIGHT = 4;
+    public final static double STRONG_CURRENT = 5.0;
+    public final static double WEAK_CURRENT = 4.0;
+    public final static double OFFSET = 100.0;
+    public final static double CLIMB_OFFSET = 45.0;
 }
diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index b3bcc6b..11e67c8 100644
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -26,6 +26,10 @@ import frc.robot.constants.swerve.DriveConstants;
 public class LinearClimb extends SubsystemBase{
     private TalonFX motor;
     private boolean calibrating = false;
+    double counter = 0;
+    double downPosition = ClimbConstants.OFFSET;
+    double upPosition = 0;
+    double climbPosition = ClimbConstants.CLIMB_OFFSET;
 
     private static PIDController pid = new PIDController(0.1, 0, 0);
     
@@ -43,6 +47,7 @@ public class LinearClimb extends SubsystemBase{
         SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown()));
         SmartDashboard.putData("Climb", new InstantCommand(() -> climb()));
         SmartDashboard.putData("Hardstop Calibrate", new InstantCommand(() -> hardstopCalibration()));
+        SmartDashboard.putData("Stop Calibrating", new InstantCommand(() -> stopCalibrating()));
         SmartDashboard.putNumber("Position", getPosition());
 
         motor.setPosition(0);
@@ -65,15 +70,15 @@ public class LinearClimb extends SubsystemBase{
     }
 
     public void goUp() {
-        setSetpoint(0);
+        setSetpoint(upPosition);
     }
 
     public void goDown() {
-        setSetpoint(0);
+        setSetpoint(downPosition);
     }
 
     public void climb() {
-        setSetpoint(ClimbConstants.CLIMB_HEIGHT);
+        setSetpoint(climbPosition);
     }
 
     public void periodic() {
@@ -83,9 +88,12 @@ public class LinearClimb extends SubsystemBase{
             motor.set(power);
             SmartDashboard.putNumber("Position", getPosition());
         }else{
+            if(counter > 250){
+                stopCalibrating();
+            }
             motor.set(0.15);
+            counter += 1;
         }
-        
     }
     public void setCurrentLimits(double limit){
         TalonFXConfiguration config = new TalonFXConfiguration();
@@ -101,6 +109,17 @@ public class LinearClimb extends SubsystemBase{
         motor.getConfigurator().apply(config);
     }
     public void hardstopCalibration(){
-        calibrating = !calibrating;
+        calibrating = true;
+        counter = 0;
+        setCurrentLimits(ClimbConstants.WEAK_CURRENT);
+    }
+    public void stopCalibrating(){
+        downPosition = motor.getPosition().getValueAsDouble();
+        upPosition = downPosition - ClimbConstants.OFFSET;
+        climbPosition = upPosition + ClimbConstants.CLIMB_OFFSET;
+        setSetpoint(downPosition);
+        calibrating = false;
+        counter = 0;
+        setCurrentLimits(ClimbConstants.STRONG_CURRENT);
     }
 }
\ No newline at end of file
-- 
2.39.5