Merge pull request #246 from thedropbears/make_measurment_block_next_state

Author: LucienMorey

controllers/floor_intake.py

@@ -47,10 +47,10 @@ def intaking(self, initial_call: bool):
     def measuring(self, initial_call):
         if initial_call:
             self.algae_measurement.measure()
-        else:
+            self.wrist.go_to_neutral()
+            self.intake_component.retract()
+        elif not self.algae_measurement.is_executing:
             self.done()
 
     def done(self) -> None:
         super().done()
-        self.wrist.go_to_neutral()
-        self.intake_component.retract()

controllers/reef_intake.py

@@ -95,7 +95,10 @@ def safing(self, initial_call: bool):
             robot_pose.translation()
         )
 
-        if distance >= self.RETREAT_DISTANCE:
+        if (
+            distance >= self.RETREAT_DISTANCE
+            and not self.algae_measurement.is_executing
+        ):
             self.done()
 
     def done(self) -> None: