Merge pull request #292 from thedropbears/make_climber_unable_to_retract_until_first_deploy

Make climber unable to retract until first deploy

Author: LeanMeanBeanMachine4774

components/climber.py

@@ -48,13 +48,13 @@ def __init__(self) -> None:
             SparkMax.PersistMode.kPersistParameters,
         )
 
-    def stop(self) -> None:
-        self.set_angle(math.radians(self.encoder_angle()))
-
     def on_disable(self) -> None:
         self.update_pid = True
         self.go_to_neutral()
 
+    def stop(self) -> None:
+        self.set_angle(math.radians(self.encoder_angle()))
+
     def stop_pid_update(self) -> None:
         self.update_pid = False
 

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]