add: individual sequende in integration_test(): mag-->mag_dist-->feeder-->pick_and_place_robot-->cell_slot-->defeeder

playgrounds/integration_test.py

@@ -107,22 +107,32 @@ class LoaderSystem:
     async def _loader_loop(self):
         while True:
 
-            await wait_for_enter()
-            # Feeding with MagDistributor
-            ###########################
-            self.logger.info("Homing the Magazin Distributor Axis...")
             await self.mag_distributor.home()
-            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 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 self.controller.pick_cell_from_slot(self.test_pickup_slot)
+            await self.controller.dropoff_cell()
+
+            # await wait_for_enter()
+            # # Feeding with MagDistributor
+            # ###########################
+            # self.logger.info("Homing the Magazin Distributor Axis...")
+            # await self.mag_distributor.home()
+            # 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
@@ -132,6 +142,7 @@ class LoaderSystem:
             # cell = Cell(id=1, status=CellStatus.WAITING)
             # await self.controller.insert_cell_to_slot(cell, self.test_drop_slot)
 
+
             # await wait_for_enter()
             # # Slot -> Defeeder
             # ##############################

robot_control/config/config.yaml

@@ -1,6 +1,6 @@
 measurement_devices:
   - id: device_1
-    position: [13, 802, 66]
+    position: [13, 804, 66]
     slots:
       - position: [0, 0, 0]
         occupied: False
@@ -36,7 +36,7 @@ feeder:
   max_num_cells: 10
 
 defeeder:
-  robot_pos: [12, 526, 72]
+  robot_pos: [12, 526, 70]
   mag_pos: 
     pos_mm: 0
     rot_deg: -90

robot_control/src/robot/controller.py

@@ -478,13 +478,14 @@ class RobotController:
             cell_v = await self.i2c.read_channel(1) * VOLTAGE_CONVERSTION_FACTOR  # Measure cell voltage
             self.gpio.set_pin(io_conf.measure_probe_oping, 0)
             logger.debug(f"Cell voltage: {cell_v}")
+            # Always move servo to eject position and back
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_end, 200)
+            await asyncio.sleep(1)  # Wait for cell to be ejected
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_start, 1000)
             if self.check_cell_voltage(cell_v, self.next_cell_id):
                 return True # Desired case!
             # Discard cell directly from feeder
             logger.info(f"Cell {self.next_cell_id} voltage({cell_v}) is bad, discarding cell")
-            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_end, 200)
-            await asyncio.sleep(1)  # Wait for cell to be ejected
-            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_start, 1000)
             try:
                 self.next_cell_id = ""
                 await self.pick_cell_from_feeder()