this should be good FINALLY (calibrating climber)

src/main/java/frc/robot/RobotContainer.java

@@ -410,10 +410,10 @@ private void configureButtonBindings() {
     controller.x().whileTrue(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE));
 
     // Climber Raise/Lower
-    controller.povUp().whileTrue(climber.raiseClimber());
-    controller.povUp().onFalse(climber.stopClimber());
-    controller.povDown().whileTrue(climber.lowerClimber());
-    controller.povDown().onFalse(climber.stopClimber());
+    // controller.povUp().whileTrue(climber.raiseClimber());
+    // controller.povUp().onFalse(climber.stopClimber());
+    // controller.povDown().whileTrue(climber.lowerClimber());
+    // controller.povDown().onFalse(climber.stopClimber());
 
     controller.povLeft().onTrue(intake.zeroEncoder());
 

src/main/java/frc/robot/subsystems/Climber.java

@@ -5,24 +5,40 @@
 import static edu.wpi.first.units.Units.Meters;
 
 import com.pathplanner.lib.util.swerve.SwerveSetpoint;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.units.AngleUnit;
+import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.W8.io.motor.MotorIO.PIDSlot;
 import frc.lib.W8.mechanisms.linear.LinearMechanism;
+import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
 import frc.robot.Constants.ClimberConstants;
 
 public class Climber extends SubsystemBase {
-  public LinearMechanism _io;
-  public Trigger homedTrigger;
-
+  private Debouncer homedDebounce = new Debouncer(0.1, DebounceType.kRising);
+  private Trigger homedTrigger;
+  private AngleUnit Degrees;
+  private VoltageUnit Volts;
+  private LinearMechanism _io;
+  RotaryMechanism climber;
   Distance goalDistance;
   SwerveSetpoint STOW;
   SwerveSetpoint setpoint;
 
   public Climber(LinearMechanism io) {
     _io = io;
+    homedTrigger =
+        new Trigger(
+            () ->
+                homedDebounce.calculate(
+                    _io.getSupplyCurrent().gte(Amps.of(ClimberConstants.HARD_STOP_CURRENT_LIMIT))));
   }
 
   // public void Position(double position) {
@@ -44,13 +60,6 @@ public boolean isAboveCurrentLimit() {
     }
   }
 
-  // public Command homeCommand()
-  //   {
-  //       return Commands.sequence(
-  //           runOnce(() -> _io.runVoltage(Volts.of(-2))),
-  //           Commands.waitUntil(() -> isAboveCurrentLimit()));
-  //   }
-
   public Command runClimber() {
     return this.runOnce(
         () ->
@@ -63,15 +72,11 @@ public Command runClimber() {
   }
 
   public Command calibrateClimber() {
-    System.out.println(ClimberConstants.CALIBRATE_VELOCITY);
-    System.out.println(ClimberConstants.ACCELERATION);
-    return this.run(
-            () ->
-                _io.runVelocity(
-                    ClimberConstants.CALIBRATE_VELOCITY,
-                    ClimberConstants.ACCELERATION,
-                    PIDSlot.SLOT_0))
-        .until(() -> isAboveCurrentLimit());
+    return Commands.sequence(
+        runOnce(() -> _io.runVoltage(Voltage.ofBaseUnits(-1, Volts))),
+        Commands.waitUntil(homedTrigger),
+        runOnce(() -> _io.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))),
+        runOnce(() -> _io.runVoltage(Voltage.ofBaseUnits(0, Volts))));
   }
 
   public Command stopClimber() {