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 1/6] 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 2/6] 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) From 5241a868611699dc3d89a7206910f5a73c365c63 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 03:34:45 -0400 Subject: [PATCH 3/6] merge fix --- .../java/frc/robot/subsystems/Climber.java | 65 ++++++++++++------- 1 file changed, 43 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 7b59f10..19ba0ea 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,6 +1,7 @@ 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; @@ -63,28 +64,6 @@ public boolean isAboveCurrentLimit() { } } - // public Command homeCommand() - // { - // return Commands.sequence( - // runOnce(() -> _io.runVoltage(Volts.of(-2))), - // Commands.waitUntil(() -> isAboveCurrentLimit())); - // } - - @Override - public void periodic() { - _io.periodic(); - - // double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - - // // The z of the Translation3D should be - // // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // // motor configs. - // Logger.recordOutput( - // "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - - // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); - } - public Command runClimber() { return this.runOnce( () -> @@ -104,6 +83,33 @@ 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() { + 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()); + } + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) @@ -114,4 +120,19 @@ public boolean nearGoalposition() { return false; } } + + @Override + public void periodic() { + _io.periodic(); + + // double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position + + // // The z of the Translation3D should be + // // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing + // // motor configs. + // Logger.recordOutput( + // "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); + + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + } } From 47b6b1d9d6bdf28da9e61a91b11f7078146ae532 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 04:12:59 -0400 Subject: [PATCH 4/6] spotless --- .../java/frc/robot/subsystems/Climber.java | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index c168683..aceec39 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -94,33 +94,6 @@ public Command lowerClimber() { .until(() -> isAboveCurrentLimit()); } - public Command stopClimber() { - return this.run( - () -> - _io.runVelocity( - DegreesPerSecond.of(0.0), ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)); - } - - public Command raiseClimber() { - 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()); - } - public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) From bb28e0d0efa4f3fe5e6bb0b51c82d31fd3c8da23 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 04:24:37 -0400 Subject: [PATCH 5/6] fix merge mistake --- .../java/frc/robot/subsystems/Climber.java | 69 ++++++++----------- 1 file changed, 28 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index aceec39..5ea1704 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,28 +1,45 @@ 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.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) { @@ -56,42 +73,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()); - } - - public Command stopClimber() { - return this.run( - () -> - _io.runVelocity( - DegreesPerSecond.of(0.0), ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)); - } - - public Command raiseClimber() { - 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(() -> _io.runVoltage(Voltage.ofBaseUnits(-1, Volts))), + Commands.waitUntil(homedTrigger), + runOnce(() -> _io.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))), + runOnce(() -> _io.runVoltage(Voltage.ofBaseUnits(0, Volts)))); } public boolean nearGoalposition() { @@ -105,6 +91,7 @@ public boolean nearGoalposition() { } } + @Override public void periodic() { _io.periodic(); @@ -119,4 +106,4 @@ public void periodic() { // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } -} +} \ No newline at end of file From 4072198cd1cea44d8dad83589de970c4b4dd7f8c Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 04:26:42 -0400 Subject: [PATCH 6/6] oops --- src/main/java/frc/robot/RobotContainer.java | 8 ++--- .../java/frc/robot/subsystems/Climber.java | 35 ++++++++++++++++--- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a0edc1..eca423a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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()); diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 5ea1704..425fabe 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,6 +1,7 @@ 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; @@ -37,9 +38,7 @@ public Climber(LinearMechanism io) { new Trigger( () -> homedDebounce.calculate( - _io - .getSupplyCurrent() - .gte(Amps.of(ClimberConstants.HARD_STOP_CURRENT_LIMIT)))); + _io.getSupplyCurrent().gte(Amps.of(ClimberConstants.HARD_STOP_CURRENT_LIMIT)))); } // public void Position(double position) { @@ -80,6 +79,33 @@ public Command calibrateClimber() { runOnce(() -> _io.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() { + 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()); + } + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) @@ -91,7 +117,6 @@ public boolean nearGoalposition() { } } - @Override public void periodic() { _io.periodic(); @@ -106,4 +131,4 @@ public void periodic() { // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } -} \ No newline at end of file +}