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 de009ce..425fabe 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/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() {