diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 5b091a0..7b59f10 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,22 +1,47 @@ 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 { + 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; + SwerveSetpoint STOW; + SwerveSetpoint setpoint; 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) { @@ -72,44 +97,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() { - 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() {