fix up climb

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 853a6c5a44fbb941d6c60f436d8ed2b5038305d4..e72affb2ca8bfaea5e7e0be91ce4a4830719331a 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -3,16 +3,23 @@ package frc.robot.constants.Climb;
 public class ClimbConstants {
 
     // CHANGE LATER
-    public final static double CLIMB_GEAR_RATIO = 9.0 / 1 * 5.0 / 1;
+    // gear ratio for converting motor rotations to linear distance
+    public final static double CLIMB_GEAR_RATIO = 1.0 / 45.0;
     public final static double MAX_VELOCITY = 1;
     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 = 42.0;
-    public final static double WEAK_CURRENT = 7.0;
     public final static double OFFSET = 100.0;
     public final static double CLIMB_OFFSET = 80.0;
 
+    // current limits (in amps)
+    // CALIBRATION: Low current while finding hardstop to prevent damage
+    // NORMAL: Moderate current for PID-controlled movement
+    // CLIMB: High current for full-power climbing
+    public final static double CALIBRATION_CURRENT = 7.0;
+    public final static double NORMAL_CURRENT = 5.0;
+    public final static double CLIMB_CURRENT = 42.0;
+
     // PID Constants
     public final static double PID_P = 0.1;
     public final static double PID_I = 0.0;
@@ -20,7 +27,7 @@ public class ClimbConstants {
     public final static double PID_TOLERANCE = 0.2;
 
     // Motor Limits
-    public final static double DEFAULT_CURRENT_LIMIT = 5.0;
+    public final static double DEFAULT_CURRENT_LIMIT = NORMAL_CURRENT;
     public final static double MIN_POWER = -0.2;
     public final static double MAX_POWER = 0.2;
     public final static double CALIBRATION_POWER = 0.15;

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 3edc773b305ac82443973802fc3f9e875a19ac62..a4e59f7282551295e679c9641a5c38beec043441 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -17,7 +17,7 @@ import frc.robot.constants.Climb.ClimbConstants;
 public class LinearClimb extends SubsystemBase {
     private final TalonFX motor;
     private boolean calibrating = false;
-    private double counter = 0;
+    private int counter = 0;
     private double downPosition = ClimbConstants.OFFSET;
     private double upPosition = 0;
     private double climbPosition = ClimbConstants.CLIMB_OFFSET;
@@ -37,7 +37,7 @@ public class LinearClimb extends SubsystemBase {
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         motor.getConfigurator().apply(config);
 
-        setCurrentLimits(ClimbConstants.DEFAULT_CURRENT_LIMIT);
+        setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
 
         SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
         SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown()));
@@ -47,6 +47,9 @@ public class LinearClimb extends SubsystemBase {
         SmartDashboard.putNumber("Position", getPosition());
 
         motor.setPosition(0);
+
+        // calibrate on startup to find hardstop
+        hardstopCalibration();
     }
 
     /**
@@ -62,10 +65,28 @@ public class LinearClimb extends SubsystemBase {
         return pid.atSetpoint();
     }
 
+    /**
+     * Returns the current position of the climb motor.
+     * 
+     * @return Position in motor rotations. Positive values move the climb mechanism
+     *         UP (toward the hardstop). Higher values = higher physical position.
+     *         Use {@link #getAsMeters()} for linear distance in meters.
+     */
     public double getPosition() {
         return motor.getPosition().getValueAsDouble();
     }
 
+    /**
+     * Returns the climb position converted to linear distance in meters.
+     * This is useful for debugging and logging.
+     * 
+     * @return Linear position in meters, calculated as:
+     *         rotations * gearRatio * 2 * PI * radius
+     */
+    public double getAsMeters() {
+        return getPosition() * ClimbConstants.CLIMB_GEAR_RATIO * 2 * Math.PI * ClimbConstants.RADIUS;
+    }
+
     public void goUp() {
         setSetpoint(upPosition);
     }
@@ -110,16 +131,17 @@ public class LinearClimb extends SubsystemBase {
     public void hardstopCalibration() {
         calibrating = true;
         counter = 0;
-        setCurrentLimits(ClimbConstants.WEAK_CURRENT);
+        setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
     }
 
     public void stopCalibrating() {
-        downPosition = motor.getPosition().getValueAsDouble() - ClimbConstants.CALIBRATION_POSITION_OFFSET;
-        upPosition = downPosition - ClimbConstants.OFFSET;
-        climbPosition = upPosition + ClimbConstants.CLIMB_OFFSET;
+        double hardstopPosition = motor.getPosition().getValueAsDouble();
+        downPosition = hardstopPosition - ClimbConstants.OFFSET;
+        climbPosition = downPosition + ClimbConstants.CLIMB_OFFSET;
+        upPosition = hardstopPosition;
         setSetpoint(downPosition);
         calibrating = false;
         counter = 0;
-        setCurrentLimits(ClimbConstants.STRONG_CURRENT);
+        setCurrentLimits(ClimbConstants.CLIMB_CURRENT);
     }
 }