Merge pull request #414 from thedropbears/redo_climber_sm

Author: LucienMorey

components/climber.py

@@ -19,7 +19,6 @@ class ClimberComponent:
 
     def __init__(self) -> None:
         self.START_ANGLE = -30.0
-        self.PRE_CLIMB_ANGLE = -20.0
         self.DEPLOY_ANGLE = 16.0
         self.RETRACT_ANGLE = -52.0
 
@@ -79,9 +78,6 @@ def set_angle(self, angle: float) -> None:
     def go_to_neutral(self) -> None:
         self.set_angle(math.radians(self.START_ANGLE))
 
-    def go_to_pre_climb(self) -> None:
-        self.set_angle(math.radians(self.PRE_CLIMB_ANGLE))
-
     def go_to_deploy(self) -> None:
         self.set_angle(math.radians(self.DEPLOY_ANGLE))
 

components/led_component.py

@@ -168,18 +168,6 @@ def climber_deploying(self, left_okay: bool, right_okay: bool) -> None:
         )
         self.keep_alive()
 
-    def climber_pre_climb(self, left_okay: bool, right_okay: bool) -> None:
-        self.pattern = LEDPattern.blink(
-            LEDPattern.steps(
-                [
-                    (0.0, Color.kGreen if left_okay else Color.kRed),
-                    (self.halfway_split, Color.kGreen if right_okay else Color.kRed),
-                ]
-            ),
-            0.125,
-        )
-        self.keep_alive()
-
     def climber_retracting(self) -> None:
         colour = Color.kRed if is_red() else Color.kBlue
         steps = []

controllers/climber.py

@@ -1,6 +1,6 @@
 import math
 
-from magicbot import StateMachine, state
+from magicbot import StateMachine, feedback, state
 
 from components.chassis import ChassisComponent
 from components.climber import ClimberComponent
@@ -15,33 +15,42 @@ class ClimberStateMachine(StateMachine):
 
     def __init__(self):
         self.has_deployed = False
+        self.seen_left = False
+        self.seen_right = False
         self.heading_to_cage = 0.0
         self.should_localize = True
 
     def on_disable(self) -> None:
-        self.has_deployed = False
         super().on_disable()
 
     def deploy(self, *, localize: bool = True) -> None:
         self.should_localize = localize
+        self.has_deployed = False
+        self.seen_right = False
+        self.seen_left = False
         self.engage("deploying", force=True)
 
     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)
     def deploying(self, initial_call) -> None:
-        self.status_lights.climber_deploying(
-            left_okay=self.climber.is_left_engaged(),
-            right_okay=self.climber.is_right_engaged(),
-        )
         if initial_call:
             self.climber.go_to_deploy()
 
+        if self.climber.is_left_engaged():
+            self.seen_left = True
+        if self.climber.is_right_engaged():
+            self.seen_right = True
+        self.status_lights.climber_deploying(
+            left_okay=self.seen_left,
+            right_okay=self.seen_right,
+        )
+
         if self.climber.is_deployed():
             self.climber.stop_pid_update()
             self.has_deployed = True
@@ -65,21 +74,9 @@ def deploying(self, initial_call) -> None:
                 )
             self.chassis.snap_to_heading(self.heading_to_cage)
 
+    @feedback
     def is_ready_to_climb(self) -> bool:
-        return self.climber.is_left_engaged() and self.climber.is_right_engaged()
-
-    def pre_climb(self) -> None:
-        self.engage("pre_climbing", force=True)
-
-    @state(must_finish=True)
-    def pre_climbing(self) -> None:
-        self.status_lights.climber_pre_climb(
-            left_okay=self.climber.is_left_engaged(),
-            right_okay=self.climber.is_right_engaged(),
-        )
-        self.climber.start_pid_update()
-        self.chassis.stop_snapping()
-        self.climber.go_to_pre_climb()
+        return self.seen_left and self.seen_right
 
     @state(must_finish=True)
     def retracting(self) -> None:

robot.py

@@ -246,13 +246,7 @@ def teleopPeriodic(self) -> None:
         if self.gamepad.getYButton():
             self.climber_state_machine.deploy()
         if self.gamepad.getAButton():
-            if (
-                self.climber_state_machine.current_state == "pre_climbing"
-                and self.climber_state_machine.is_ready_to_climb()
-            ) or self.climber_state_machine.current_state == "retracting":
-                self.climber_state_machine.retract()
-            else:
-                self.climber_state_machine.pre_climb()
+            self.climber_state_machine.retract()
 
         if self.gamepad.getBButton():
             self.reef_intake.done()
@@ -315,13 +309,7 @@ def testPeriodic(self) -> None:
             self.climber_state_machine.deploy_without_localization()
 
         if self.gamepad.getAButton():
-            if (
-                self.climber_state_machine.current_state == "pre_climbing"
-                and self.climber_state_machine.is_ready_to_climb()
-            ) or self.climber_state_machine.current_state == "retracting":
-                self.climber_state_machine.retract()
-            else:
-                self.climber_state_machine.pre_climb()
+            self.climber_state_machine.retract()
 
         if self.gamepad.getXButton():
             self.coral_depositor_component.deposit()