moved climber saftey functionality into the state machine

Author: DannyPomeranian

components/climber.py

@@ -18,7 +18,6 @@ class ClimberComponent:
     winch_voltage = tunable(12.0)
 
     def __init__(self) -> None:
-        self.has_deployed = False
         self.START_ANGLE = -30.0
         self.DEPLOY_ANGLE = 11.0
         self.RETRACT_ANGLE = -52.0
@@ -51,7 +50,6 @@ def __init__(self) -> None:
 
     def on_disable(self) -> None:
         self.update_pid = True
-        self.has_deployed = False
         self.go_to_neutral()
 
     def stop(self) -> None:
@@ -73,8 +71,7 @@ def go_to_deploy(self) -> None:
         self.set_angle(math.radians(self.DEPLOY_ANGLE))
 
     def go_to_retract(self) -> None:
-        if self.has_deployed:
-            self.set_angle(math.radians(self.RETRACT_ANGLE))
+        self.set_angle(math.radians(self.RETRACT_ANGLE))
 
     @feedback
     def raw_encoder_val(self) -> float:

controllers/climber.py

@@ -14,13 +14,18 @@ class ClimberStateMachine(StateMachine):
     chassis: ChassisComponent
 
     def __init__(self):
+        self.has_deployed = False
         self.heading_to_cage = 0.0
 
+    def on_disable(self) -> None:
+        self.has_deployed = False
+
     def deploy(self) -> None:
         self.engage("deploying")
 
     def retract(self) -> None:
-        self.engage("retracting", force=True)
+        if self.has_deployed:
+            self.engage("retracting", force=True)
 
     @state(first=True, must_finish=True)
     def deploying(self, initial_call) -> None:
@@ -30,6 +35,7 @@ def deploying(self, initial_call) -> None:
 
         if self.climber.is_deployed():
             self.climber.stop_pid_update()
+            self.has_deployed = True
 
         cage_positions = cage_pos(is_red())
         closest_cage_position = cage_positions[0]