feat: integrate MagDistributor for cell handling in integration test

playgrounds/integration_test.py

@@ -10,6 +10,7 @@ from robot_control.src.api.i2c_handler import I2C, MockI2C
 from robot_control.src.vendor.mcp3428 import MCP3428
 from robot_control.src.robot.pump_controller import PumpController
 from robot_control.src.api.gpio import PiGPIO, MockGPIO
+from robot_control.src.robot.mag_distributor import MagDistributor
 
 
 """
@@ -47,8 +48,13 @@ class LoaderSystem:
         # Initialize pump controller
         self.pump_controller = PumpController(self.config, self.gpio)
 
+        # Initialize magazine distributor
+        self.mag_distributor = MagDistributor(self.config, self.gpio)
+
         i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
         self.i2c = I2C(i2c_device_class)
+        self.i2c.initialize()
+        self.logger.info(f"I2C initialized with {i2c_device_class.__name__}")
 
         self.feeder_queue: asyncio.Queue[int] = asyncio.Queue(self.config.feeder.max_num_cells)
         self.defeeder_queue: asyncio.Queue[DefeederMagazineConfig]  = asyncio.Queue(self.config.defeeder.max_num_cells)
@@ -69,7 +75,8 @@ class LoaderSystem:
         self.vision.initialize()
 
         # Test stuff
-        self.test_slot = self.config.measurement_devices[0].slots[3]
+        self.test_drop_slot = self.config.measurement_devices[0].slots[2]
+        self.test_pickup_slot = self.config.measurement_devices[0].slots[3]
 
         # Use mock I2C device if debug is enabled
         self.i2c.initialize()
@@ -101,18 +108,45 @@ class LoaderSystem:
 
     async def _loader_loop(self):
         while True:
+
             await wait_for_enter()
+            # Feeding with MagDistributor
+            ###########################
+            self.logger.info("Picking up a cell from magazine and placing it into feeder using MagDistributor...")
+            self.mag_distributor.mag_to_feeder()
+            await self.feeder_queue.put(1)
+            self.logger.info("Done.")
+
+            await wait_for_enter()
+            # Prepare Feeder Cell
+            ##############################
+            self.logger.info("Preparing feeder cell...")
             await self.controller.prepare_feeder_cell()
+            self.logger.info("Done.")
 
+            await wait_for_enter()
+            # Feeder -> Slot
+            ##############################
+            self.logger.info("Picking up a cell from feeder and placing it into slot...")
+            await self.controller.pick_cell_from_feeder()
+            cell = Cell(id=1, status=CellStatus.WAITING)
+            await self.controller.insert_cell_to_slot(cell, self.test_drop_slot)
 
             await wait_for_enter()
-            self.logger.info("Starting movement sequence...")
-            await self.controller.pick_cell_from_slot(self.test_slot)
+            # Slot -> Defeeder
+            ##############################
+            self.logger.info("Picking up a cell from slot and placing it into defeeder...")
+            await self.controller.pick_cell_from_slot(self.test_pickup_slot)
             await self.controller.dropoff_cell()
+            self.logger.info("Done.")
+
+            await wait_for_enter()
+            # Defeeding with MagDistributor
+            ###########################
+            self.logger.info("Defeeding a cell from feeder to magazine using MagDistributor...")
+            self.mag_distributor.defeeder_to_mag(self.config.defeeder_magazines[0])
+            self.logger.info("Done.")
 
-            await self.controller.pick_cell_from_feeder()
-            cell = Cell(id=1, status=CellStatus.WAITING)
-            await self.controller.insert_cell_to_slot(cell, self.test_slot)
             self.logger.info("\nPress Enter to repeat sequence (or Ctrl+C to exit)...")
 
     async def cleanup(self):