Merge pull request #239 from thedropbears/add_intake_short_circuit

Author: LucienMorey

controllers/floor_intake.py

@@ -26,6 +26,11 @@ def intake(self) -> None:
 
     @state(first=True, must_finish=True)
     def intaking(self, initial_call: bool):
+        # This is different to the regular business logic below because these two things are mutually exclusive unless we have already got something
+        if initial_call and self.injector_component.has_algae():
+            self.done()
+            return
+
         if self.injector_component.has_algae():
             self.next_state("measuring")
             return

controllers/reef_intake.py

@@ -54,8 +54,13 @@ def intaking(self, initial_call: bool):
                 self.done()
                 return
 
+            if self.injector_component.has_algae():
+                self.done()
+                return
+
         if self.injector_component.has_algae():
             self.next_state("safing")
+            return
 
         nearest_tag_pose = (game.nearest_reef_tag(current_pose))[1]
         self.rotation_lock = nearest_tag_pose.rotation()

robot.py

@@ -277,6 +277,7 @@ def testPeriodic(self) -> None:
         self.climber.execute()
         self.coral_placer_component.execute()
         self.shooter_component.execute()
+        self.injector_component.execute()
         if self.gamepad.getStartButton():
             self.vision.zero_servo_()
         else: