From 44d933c5d51e16630c63dc8d7cc0fd495f6e1821 Mon Sep 17 00:00:00 2001 From: Area5188 Date: Thu, 12 Mar 2026 18:06:19 -0400 Subject: [PATCH 1/4] this should be good FINALLY --- .../java/frc/robot/subsystems/Climber.java | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4aade98..a5b0589 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -4,24 +4,38 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Volts; +import com.pathplanner.lib.util.swerve.SwerveSetpoint; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.AngleUnit; +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.wpilibj.Timer; 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; import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { + private final RotaryMechanism _climber; + private Trigger homedTrigger; + private AngleUnit Degrees; private LinearMechanism _io; + RotaryMechanism climber; Distance goalDistance; + SwerveSetpoint STOW; + SwerveSetpoint setpoint; public Climber(LinearMechanism io) { _io = io; + _climber = climber; } // public void Position(double position) { @@ -69,6 +83,14 @@ public Command runClimber() { PIDSlot.SLOT_0)); } + public Command calibrateClimber() { + return Commands.sequence( + runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(-1, Volts))), + Commands.waitUntil(homedTrigger), + runOnce(() -> _climber.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))), + runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(0, Volts)))); + } + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) From 53c5c718efa8f4782d0bf606708257cb97867d0c Mon Sep 17 00:00:00 2001 From: Area5188 Date: Thu, 12 Mar 2026 18:18:54 -0400 Subject: [PATCH 2/4] mayhaps i did something wrong --- src/main/java/frc/robot/subsystems/Climber.java | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 5501c47..7b59f10 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,18 +1,16 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Meters; import com.pathplanner.lib.util.swerve.SwerveSetpoint; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; +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.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,8 +22,10 @@ public class Climber extends SubsystemBase { private final RotaryMechanism _climber; + 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; @@ -35,6 +35,13 @@ public class Climber extends SubsystemBase { public Climber(LinearMechanism io) { _io = io; _climber = climber; + homedTrigger = + new Trigger( + () -> + homedDebounce.calculate( + _climber + .getSupplyCurrent() + .gte(Amps.of(ClimberConstants.HARD_STOP_CURRENT_LIMIT)))); } // public void Position(double position) { From 09dce17dd8dddf5b2381eb4672aa12799d258f93 Mon Sep 17 00:00:00 2001 From: rhit-collinkn <95197995+rhit-collinkn@users.noreply.github.com> Date: Sat, 14 Mar 2026 22:18:36 -0400 Subject: [PATCH 3/4] readding prev code --- .../java/frc/robot/subsystems/Climber.java | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 7b59f10..1441265 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -104,6 +104,41 @@ public Command calibrateClimber() { runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(0, Volts)))); } + public Command stopClimber() { + return this.run( + () -> + _io.runVelocity( + DegreesPerSecond.of(0.0), ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)); + } + + public Command raiseClimber() { + System.out.println(ClimberConstants.CRUISE_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.CRUISE_VELOCITY, + ClimberConstants.ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + + public Command lowerClimber() { + System.out.println(ClimberConstants.LOWER_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.LOWER_VELOCITY, ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + return Commands.sequence( + runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(-1, Volts))), + Commands.waitUntil(homedTrigger), + runOnce(() -> _climber.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))), + runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(0, Volts)))); + } + + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) From 9290300a53e09beeda9fc81a8ad01b765d99ce70 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Sat, 14 Mar 2026 22:25:57 -0400 Subject: [PATCH 4/4] Revert "readding prev code" This reverts commit 09dce17dd8dddf5b2381eb4672aa12799d258f93. revert --- .../java/frc/robot/subsystems/Climber.java | 35 ------------------- 1 file changed, 35 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 1441265..7b59f10 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -104,41 +104,6 @@ public Command calibrateClimber() { runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(0, Volts)))); } - public Command stopClimber() { - return this.run( - () -> - _io.runVelocity( - DegreesPerSecond.of(0.0), ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)); - } - - public Command raiseClimber() { - System.out.println(ClimberConstants.CRUISE_VELOCITY); - System.out.println(ClimberConstants.ACCELERATION); - return this.run( - () -> - _io.runVelocity( - ClimberConstants.CRUISE_VELOCITY, - ClimberConstants.ACCELERATION, - PIDSlot.SLOT_0)) - .until(() -> isAboveCurrentLimit()); - } - - public Command lowerClimber() { - System.out.println(ClimberConstants.LOWER_VELOCITY); - System.out.println(ClimberConstants.ACCELERATION); - return this.run( - () -> - _io.runVelocity( - ClimberConstants.LOWER_VELOCITY, ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)) - .until(() -> isAboveCurrentLimit()); - return Commands.sequence( - runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(-1, Volts))), - Commands.waitUntil(homedTrigger), - runOnce(() -> _climber.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))), - runOnce(() -> _climber.runVoltage(Voltage.ofBaseUnits(0, Volts)))); - } - - public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters)