yay

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index aef16c6a7ed317efdca6b83c53d93edaaa20b116..ddbbf23d6a4ca9e2ca6970167fd71063365887e9 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -1,13 +1,16 @@
 package frc.robot.constants.Climb;
 
+import edu.wpi.first.math.util.Units;
+
 public class ClimbConstants {
 
     // CHANGE LATER
     // gear ratio for converting motor rotations to linear distance
     public final static double CLIMB_GEAR_RATIO = 1.0 / 45.0;
-    public final static double RADIUS = 0.3;
-    public final static double OFFSET = 100.0;
-    public final static double CLIMB_OFFSET = 80.0;
+    public final static double WHEEL_RADIUS = Units.inchesToMeters(0.334);
+    public final static double BOTTOM_POSITION = Units.inchesToMeters(-8);
+    public final static double CLIMB_POSITION = Units.inchesToMeters(-6);
+    public final static double UP_POSITION = 0.0;
 
     // current limits (in amps)
     // CALIBRATION: Low current while finding hardstop to prevent damage

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 3fa622e7dd5cf5aab4019cba33b77afa486a6a43..f64e007926e417e04b3d4397886224ea7947e3ae 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -17,9 +18,9 @@ import frc.robot.constants.Climb.ClimbConstants;
 public class LinearClimb extends SubsystemBase {
     private final TalonFX motor;
     private boolean calibrating = false;
-    private double downPosition = ClimbConstants.OFFSET;
-    private double upPosition = 0;
-    private double climbPosition = ClimbConstants.CLIMB_OFFSET;
+    private double downPosition = ClimbConstants.BOTTOM_POSITION;
+    private double upPosition = ClimbConstants.UP_POSITION;
+    private double climbPosition = ClimbConstants.CLIMB_POSITION;
 
     private final PIDController pid = new PIDController(
             ClimbConstants.PID_P,
@@ -59,7 +60,9 @@ public class LinearClimb extends SubsystemBase {
     public void setSetpoint(double setpoint) {
         pid.setSetpoint(setpoint);
     }
-
+    /**
+     * @return when the position is within 0.2 rotations
+     */
     public boolean atSetPoint() {
         return pid.atSetpoint();
     }
@@ -83,36 +86,43 @@ public class LinearClimb extends SubsystemBase {
      *         rotations * gearRatio * 2 * PI * radius
      */
     public double getAsMeters() {
-        return getPosition() * ClimbConstants.CLIMB_GEAR_RATIO * 2 * Math.PI * ClimbConstants.RADIUS;
+        return getPosition() * ClimbConstants.CLIMB_GEAR_RATIO * 2 * Math.PI * ClimbConstants.WHEEL_RADIUS;
     }
-
+    /**
+     * goes to the up position
+     */
     public void goUp() {
-        setSetpoint(upPosition);
+        setSetpoint(Units.radiansToRotations(upPosition / ClimbConstants.WHEEL_RADIUS));
     }
-
+    /**
+     * goes to the down position
+     */
     public void goDown() {
-        setSetpoint(downPosition);
+        setSetpoint(Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS));
     }
-
+    /**
+     * goes to the climb position
+     */
     public void climb() {
-        setSetpoint(climbPosition);
+        setSetpoint(Units.radiansToRotations(climbPosition / ClimbConstants.WHEEL_RADIUS));
     }
 
     @Override
     public void periodic() {
-        if (calibrating) {
+        // if it is not calibrating, do normal stuff
+        if (!calibrating) {
             double power = pid.calculate(motor.getPosition().getValueAsDouble());
             power = MathUtil.clamp(power, ClimbConstants.MIN_POWER, ClimbConstants.MAX_POWER);
             motor.set(power);
         } else {
-            if (!calibrating) {
-                stopCalibrating();
-            }
             motor.set(ClimbConstants.CALIBRATION_POWER);
         }
         SmartDashboard.putNumber("Position", getPosition());
     }
-
+    /**
+     * sets supply and stator current limits
+     * @param limit the current limit for stator and supply current
+     */
     public void setCurrentLimits(double limit) {
         TalonFXConfiguration config = new TalonFXConfiguration();
 
@@ -125,18 +135,18 @@ public class LinearClimb extends SubsystemBase {
 
         motor.getConfigurator().apply(config);
     }
-
+    /**
+     * Sets the motor to a slow speed until it hits the hard stop
+     */
     public void hardstopCalibration() {
         calibrating = true;
         setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
     }
-
+    /**
+     * stops calibration and sets current limits to normal. 
+     */
     public void stopCalibrating() {
-        double hardstopPosition = motor.getPosition().getValueAsDouble();
-        downPosition = hardstopPosition - ClimbConstants.OFFSET;
-        climbPosition = downPosition + ClimbConstants.CLIMB_OFFSET;
-        upPosition = hardstopPosition;
-        setSetpoint(downPosition);
+        motor.setPosition(downPosition);
         calibrating = false;
         setCurrentLimits(ClimbConstants.CLIMB_CURRENT);
     }