diff --git a/src/main/java/frc/robot/commands/climber_commands/ClimberPullDown.java b/src/main/java/frc/robot/commands/climber_commands/ClimberPullDown.java index 99fec291..183fd765 100644 --- a/src/main/java/frc/robot/commands/climber_commands/ClimberPullDown.java +++ b/src/main/java/frc/robot/commands/climber_commands/ClimberPullDown.java @@ -37,6 +37,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_climber.getRightRPM() < ClimberConstants.CLIMBER_RPM && m_climber.getLeftRPM() < ClimberConstants.CLIMBER_RPM; + return m_climber.isClimberIn(); } } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 7cb2501c..42c300db 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -4,11 +4,14 @@ package frc.robot.subsystems; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkLimitSwitch; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder; - import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -22,12 +25,16 @@ public class Climber extends SubsystemBase { RelativeEncoder climbEncoderL; double speed; double voltage; + SparkLimitSwitch limitSwitchR; + SparkLimitSwitch limitSwitchL; /** Creates a new Climber. */ public Climber() { climbMotorR = new CANSparkMax(ClimberConstants.CLIMBER_MOTOR_RIGHT, MotorType.kBrushless); climbMotorL = new CANSparkMax(ClimberConstants.CLIMBER_MOTOR_LEFT, MotorType.kBrushless); climbEncoderR = climbMotorR.getEncoder(SparkRelativeEncoder.Type.kQuadrature, 4098); climbEncoderL = climbMotorL.getEncoder(SparkRelativeEncoder.Type.kQuadrature, 4098); + limitSwitchR = climbMotorR.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); + limitSwitchL = climbMotorL.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); climbMotorL.setIdleMode(IdleMode.kBrake); climbMotorR.setIdleMode(IdleMode.kBrake); } @@ -54,4 +61,8 @@ public double getLeftRPM(){ return climbEncoderL.getVelocity(); } + public boolean isClimberIn(){ + return limitSwitchR.isPressed() && limitSwitchL.isPressed(); +} + }