added unsafe distance after intaking algae

controllers/reef_intake.py

@@ -16,6 +16,8 @@ class ReefIntake(StateMachine):
     L2_INTAKE_ANGLE = tunable(math.radians(-40.0))
     L3_INTAKE_ANGLE = tunable(math.radians(-10.0))
 
+    RETREAT_DISTANCE = tunable(0.6)
+
     def __init__(self):
         self.last_l3 = False
 
@@ -24,9 +26,12 @@ def intake(self) -> None:
 
     @state(first=True, must_finish=True)
     def intaking(self, initial_call: bool):
-        if self.algae_manipulator_component.has_algae():
+        if self.algae_manipulator_component.has_algae() and initial_call:
             self.done()
             return
+        elif self.algae_manipulator_component.has_algae():
+            self.next_state("safing")
+            return
 
         current_is_L3 = self.is_L3()
 
@@ -39,6 +44,18 @@ def intaking(self, initial_call: bool):
 
         self.algae_manipulator_component.intake()
 
+    @state(must_finish=True)
+    def safing(self, initial_call: bool):
+        if initial_call:
+            self.origin_robot_pose = self.chassis.get_pose()
+        robot_pose = self.chassis.get_pose()
+
+        distance = self.origin_robot_pose.translation() - robot_pose.translation()
+
+        if distance.norm() >= self.RETREAT_DISTANCE:
+            self.done()
+            return
+
     @feedback
     def is_L3(self) -> bool:
         return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose()))