feat: retry defeeding next time; add max cell limits to magazine handling; rename variables for clarity and improve error handling

main.py

@@ -108,8 +108,8 @@ class LoaderSystem:
                 # Fill the next free slot (returns True if a cell was placed)
                 if not await self.controller.fill_next_free_slot():
                     break
-            # Check for completed measurements and sort cell
-            await self.controller.process_finished_measurement()
+            # Check for completed measurements and sort cells
+            await self.controller.process_finished_measurements()
 
     async def _queue_monitor_loop(self):
         """

robot_control/src/robot/controller.py

@@ -54,7 +54,7 @@ class RobotController:
         
         # Calculate total slots and initialize work queue
         self.total_slots = sum(len(device.slots) for device in self.devices)
-        self.work_queue: List[MeasurementResult] = []
+        self.pending_measurements: List[MeasurementResult] = []
 
         self.gripper_occupied = False
         self.suction_state = False
@@ -73,7 +73,7 @@ class RobotController:
             self.mqtt_handler.register_device(
                 device_id=device.id,
                 num_slots=len(device.slots),
-                callback=lambda measurement_result: self.work_queue.append(measurement_result)
+                callback=lambda measurement_result: self.pending_measurements.append(measurement_result)
             )
 
     async def perform_homing(self):
@@ -150,6 +150,14 @@ class RobotController:
         except IndexError as e:
             logger.error(f"Slot {slot_id} not found in device {device_id}")
             return None
+    
+    def get_mag_by_name(self, name: str) -> Optional[DefeederMagazineConfig]:
+        """Retrieve a defeeder magazine by its name."""
+        for mag in self.defeeder_magazines:
+            if mag.name.lower() == name.lower():
+                return mag
+        logger.error(f"Magazine '{name}' not found")
+        return None
 
     def get_next_free_slot(self) -> Optional[SlotConfig]:
         """
@@ -165,37 +173,47 @@ class RobotController:
         logger.warning("No free slots available")
         return None
 
-    async def process_finished_measurement(self):
+    async def process_finished_measurements(self):
         """
-        Process the next finished measurement from the work queue.
-        Picks the cell from the slot and sorts it based on result.
+        Process the all finished measurements from the work queue.
+        Picks the cell from the slot and adds it to the pending buffer for sorting.
         """
-        if not self.work_queue:
+        if not self.pending_measurements:
             logger.info("No finished measurements in queue")
-            return
-        
-        measurement_result = self.work_queue.pop(0)
-        cell = self.get_cell_by_id(measurement_result.cell_id)
-        try:
-            cell_status = CellStatus(measurement_result.status)
-        except ValueError as e:
-            logger.error(f"Measurement ({measurement_result}) could not be processed. Invalid cell status '{measurement_result.status}'")
-            return
-
-        waiting_slot = self.get_slot_by_id(measurement_result.device_id, measurement_result.slot_id)
-        if not waiting_slot:
-            logger.error(f"Measurement ({measurement_result}) could not be processed.")
-            return
-        await self.pick_cell_from_slot(waiting_slot)
+            return        
 
-        if not cell: # Cell not found, create new
-            cell = Cell(measurement_result.cell_id, cell_status, health=measurement_result.health)
-            self.cells[measurement_result.cell_id] = cell
-        else:
-            cell.health = measurement_result.health
-            cell.status = cell_status
+        remaining = []
+        for measurement in self.pending_measurements:            
+            cell = self.get_cell_by_id(measurement.cell_id)
+            try:
+                cell_status = CellStatus(measurement.status)
+            except ValueError as e:
+                logger.error(f"Measurement ({measurement}) could not be processed. Invalid cell status '{measurement.status}'")
+                return
+
+            waiting_slot = self.get_slot_by_id(measurement.device_id, measurement.slot_id)
+            if not waiting_slot:
+                logger.error(f"Measurement ({measurement}) could not be processed. No slot found for device {measurement.device_id} and slot {measurement.slot_id}")
+                return
+            if not await self.pick_cell_from_slot(waiting_slot):
+                remaining.append(measurement)
+                continue
+
+            if not cell: # Cell not found, create new
+                cell = Cell(measurement.cell_id, cell_status, health=measurement.health)
+                self.cells[measurement.cell_id] = cell
+            else:
+                cell.health = measurement.health
+                cell.status = cell_status
+
+            # Sort the cell and retry next time if unsuccessful
+            success = await self.sort_cell(cell)
+            if not success:
+                remaining.append(measurement)
+
+        self.pending_measurements = remaining
 
-        await self.sort_cell(cell)
+        
 
     async def pick_cell_from_feeder(self):
         """
@@ -327,7 +345,7 @@ class RobotController:
             self.activate_endeffector()
             if not self.suction_state:
                 logger.error("Suction state is off, cannot pick cell")
-                return False
+                return None
             self.gripper_occupied = True
             cell_id = slot.cell_id
             slot.occupied = False
@@ -354,15 +372,24 @@ class RobotController:
         if cell.id in self.cells:
             del self.cells[cell.id] # Remove cell from our database TODO [SG]: Should we keep it for history?
 
+        dropoff_mag = None
+
+        # If cell is in error state, drop it off in the error magazine (if available)
         if cell.status is CellStatus.ERROR:
-            cell.health = 0 # will be dropped off in the lowest grade
+            cell.health = 0  # will be dropped off in the "error" magazine
+            dropoff_mag = self.get_mag_by_name("error")  # Ensure error magazine exists
+            if not dropoff_mag:  
+                logger.warning(f"No error magazine configured, cannot sort cell {cell.id} to error")
+            
         for mag in self.defeeder_magazines:
             if cell.health >= mag.health_range[0] and cell.health <= mag.health_range[1]:
-                await self.dropoff_cell(mag)
-                logger.info(f"Cell {cell.id} sorted to magazine {mag.name}")
-                return True
-        logger.error(f"No suitable magazine found for cell {cell.id} with capacity {cell.health}")
-        return False
+                dropoff_mag = mag
+
+        if not dropoff_mag:
+            logger.error(f"No suitable magazine found for cell {cell.id} with capacity {cell.health}")
+            return False
+        
+        return await self.dropoff_cell(dropoff_mag)
     
     async def dropoff_cell(self, defeeder_mag: Optional[DefeederMagazineConfig] = None):
         """
@@ -376,6 +403,12 @@ class RobotController:
         # If no defeeddefeeder_mag_idx is given, use the last one in the list
         if defeeder_mag is None:
             defeeder_mag = self.defeeder_magazines[-1]
+
+        # Check if the magazine has space left
+        if defeeder_mag.max_num_cells is not None and defeeder_mag.current_num_cells >= defeeder_mag.max_num_cells:
+            logger.error(f"Magazine '{defeeder_mag.name}' is full, cannot drop off cell")
+            return False
+
         if self.defeeder_queue is not None:
             try:
                 await self.defeeder_queue.put(defeeder_mag)
@@ -384,9 +417,9 @@ class RobotController:
 
         pos = self.config.defeeder.robot_pos
         safe_pos = (pos[0], pos[1], self.config.movement.safe_height)
-        logger.info(f"Moving to dropoff (safe) {safe_pos}...")
+        logger.debug(f"Moving to dropoff (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
-        logger.info(f"Moving to dropoff position {pos}...")
+        logger.debug(f"Moving to dropoff position {pos}...")
         await self.movement.move_to_position(*pos)
         # Release cell
         if not self.deactivate_endeffector():
@@ -394,9 +427,9 @@ class RobotController:
         self.gripper_occupied = False
 
         # Move back to safe height
-        logger.info(f"Moving to dropoff position (safe) {safe_pos}...")
+        logger.debug(f"Moving to dropoff position (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
-        logger.info(f"Cell dropped off at defeeder")
+        logger.info(f"Cell dropped off at defeeder {defeeder_mag.name}")
 
     def check_cell_voltage(self, voltage, cell_id_str = ""):
         """
@@ -407,7 +440,7 @@ class RobotController:
             logger.info(f"Cell {cell_id_str} voltage too low, discarding cell")
             return False
         if voltage < 0:
-            logger.info(f"Cell {cell_id_str} has wrong polarity, discarding cell")
+            logger.warning(f"Cell {cell_id_str} has wrong polarity, discarding cell")
             return False
         logger.info(f"Cell {cell_id_str} voltage({voltage}) is good")
         return True

robot_control/src/robot/mag_distributor.py

@@ -43,7 +43,7 @@ class MagDistributor:
         self.current_magazine = 0
 
         # Track magazines already checked for picking
-        self.empty_magazines : set[int] = set()
+        self.empty_feeder_magazines : set[int] = set()
 
         self.logger = logging.getLogger(__name__)
 
@@ -108,11 +108,18 @@ class MagDistributor:
             self.gpio.do_step(self.rot_dir_pin, self.rot_step_pin, rot_steps, step_delay, rot_dir)
             self.curr_rot_deg = rot_target
 
-    def reset_empty_magazines(self):
+    def reset_feeder_magazines(self):
         """
         Reset the set of magazines already checked for picking.
         """
-        self.empty_magazines.clear()
+        self.empty_feeder_magazines.clear()
+
+    def reset_defeeder_magazines(self):
+        """
+        Reset the set of magazines for defeeding.
+        """
+        for mag in self.config.feeder_magazines:
+            mag.current_num_cells = 0
 
     def mag_to_feeder(self, magazine_id: Optional[int] = None):
         """
@@ -125,7 +132,7 @@ class MagDistributor:
         else:
             # Only try magazines that have not been checked yet
             mags_to_try = [i for i in (list(range(self.current_magazine, len(magazines))) + list(range(0, self.current_magazine)))
-                           if i not in self.empty_magazines]
+                           if i not in self.empty_feeder_magazines]
             
         # Check if cell can be picked from magazines, if not add to empty magazines and continue
         for mag_id in mags_to_try:
@@ -140,15 +147,21 @@ class MagDistributor:
                 return
             else:
                 self.logger.warning(f"Failed to pick cell from magazine {mag_id}. Trying next magazine.")
-                self.empty_magazines.add(mag_id)
+                self.empty_feeder_magazines.add(mag_id)
                 continue
-        self.logger.warning("No more available magazines to pick from. All attempts failed.")
+        self.logger.error("No more available magazines to pick from. All attempts failed.")
 
     def defeeder_to_mag(self, magazine: DefeederMagazineConfig):
         """
         Move a cell from the defeeder to a specific magazine.
         Includes checks for valid magazine_id and sensor confirmation.
         """
+        if magazine.current_num_cells >= magazine.max_num_cells:
+            self.logger.error(f"Magazine '{magazine.name}' is full, cannot move cell from defeeder.")
+            return
+        
+        magazine.current_num_cells += 1  # Increment cell count in the magazine
+
         # Move to defeeder position
         pos = self.config.defeeder.mag_pos
         self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)

robot_control/src/utils/config.py

@@ -39,6 +39,7 @@ class MagDistPosition(BaseModel):
 class MagazineConfig(BaseModel):
     mag_pos: MagDistPosition
     max_num_cells: int = Field(default=10, ge=0)
+    current_num_cells: int = Field(default=0, ge=0)
 
 class DefeederMagazineConfig(MagazineConfig):
     name: str