move is_ready_to_climb check to state machine

Author: L9-bms

controllers/climber.py

@@ -34,7 +34,7 @@ def deploy_without_localization(self) -> None:
         self.deploy(localize=False)
 
     def retract(self) -> None:
-        if self.has_deployed:
+        if self.has_deployed and self.is_ready_to_climb():
             self.engage("retracting", force=True)
 
     @state(first=True, must_finish=True)

robot.py

@@ -245,7 +245,7 @@ def teleopPeriodic(self) -> None:
 
         if self.gamepad.getYButton():
             self.climber_state_machine.deploy()
-        if self.gamepad.getAButton() and self.climber_state_machine.is_ready_to_climb():
+        if self.gamepad.getAButton():
             self.climber_state_machine.retract()
 
         if self.gamepad.getBButton():