diff --git a/components/climber.py b/components/climber.py index fc4b9c5..bda2791 100644 --- a/components/climber.py +++ b/components/climber.py @@ -1,27 +1,49 @@ from magicbot import tunable, will_reset_to -from rev import SparkMax, SparkMaxConfig +from rev import LimitSwitchConfig, SparkMax, SparkMaxConfig from ids import SparkId from utilities.rev import configure_spark_reset_and_persist class ClimberComponent: - setpoint = will_reset_to(0.0) - climb_speed = tunable(0.2) + MAX_FORWARD_EXTENSION, MAX_REVERSE_EXTENSION = 120, -120 + + current_climber_speed = will_reset_to(0.0) + forward_climber_speed, reverse_climber_speed = tunable(0.8), tunable(0.8) def __init__(self): # create motor with correct forward direction sparkmax controller self.motor = SparkMax(SparkId.CLIMBER, SparkMax.MotorType.kBrushless) + self.forward_limit_switch = self.motor.getForwardLimitSwitch() + self.reverse_limit_switch = self.motor.getReverseLimitSwitch() + self.encoder = self.motor.getEncoder() + config = SparkMaxConfig() + config.inverted(True) config.setIdleMode(SparkMaxConfig.IdleMode.kBrake) + + config.limitSwitch.forwardLimitSwitchType( + LimitSwitchConfig.Type.kNormallyOpen + ).forwardLimitSwitchTriggerBehavior( + LimitSwitchConfig.Behavior.kStopMovingMotor + ).reverseLimitSwitchType( + LimitSwitchConfig.Type.kNormallyOpen + ).reverseLimitSwitchTriggerBehavior(LimitSwitchConfig.Behavior.kStopMovingMotor) + + config.softLimit.forwardSoftLimit( + self.MAX_FORWARD_EXTENSION + ).forwardSoftLimitEnabled(True).reverseSoftLimit( + self.MAX_REVERSE_EXTENSION + ).reverseSoftLimitEnabled(True) + configure_spark_reset_and_persist(self.motor, config) def deploy(self): - self.setpoint = -self.climb_speed + self.current_climber_speed = self.forward_climber_speed - def climb(self): - self.setpoint = self.climb_speed + def retract(self): + self.current_climber_speed = self.reverse_climber_speed def execute(self): - self.motor.set(self.setpoint) + self.motor.set(self.current_climber_speed) diff --git a/robot.py b/robot.py index 8bab46a..94a7871 100644 --- a/robot.py +++ b/robot.py @@ -194,6 +194,7 @@ def testInit(self) -> None: def testPeriodic(self) -> None: allowed_to_drive = self.gamepad.getRightBumperButton() + if allowed_to_drive: # Set max speed max_speed = self.lower_max_speed @@ -216,11 +217,11 @@ def testPeriodic(self) -> None: self.shooter.shoot() self.intake.intake() - if self.gamepad.getAButton(): + if self.gamepad.getAButtonPressed(): self.climber.deploy() - if self.gamepad.getYButton(): - self.climber.climb() + if self.gamepad.getBButtonPressed(): + self.climber.retract() self.shooter.execute() self.climber.execute()