feat: refactor magazine handling and update configuration; replace capacity with health metrics

main.py

@@ -1,7 +1,7 @@
 import asyncio
 import logging
 from robot_control.src.robot.controller import RobotController
-from robot_control.src.utils.config import ConfigParser
+from robot_control.src.utils.config import ConfigParser, DefeederMagazineConfig
 from robot_control.src.vision.datamatrix import DataMatrixReader
 from robot_control.src.api.i2c_handler import I2C, MockI2C
 from robot_control.src.vendor.mcp3428 import MCP3428
@@ -45,8 +45,8 @@ class LoaderSystem:
         # Create feeder and defeeder queues used for async handling of cell
         # Magazine -> MagDist -> Feeder   -> Robot
         # Magazine <- MagDist <- Defeeder <- Robot
-        self.feeder_queue: asyncio.Queue[int] = asyncio.Queue(self.config.feeder.max_capacity)
-        self.defeeder_queue: asyncio.Queue[int]  = asyncio.Queue(self.config.defeeder.max_capacity)
+        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)
 
         # Pass all hardware interfaces to the controller
         self.controller = RobotController(
@@ -82,7 +82,7 @@ class LoaderSystem:
         """
         while True:
             try:
-                readings = await self.i2c.read_channels([1, 3, 4])
+                readings = await self.i2c.read_channels([3, 4]) # channel 1 is handled on demand in Controller
                 for channel, value in readings.items():
                     self.logger.debug(f"Channel {channel} reading: {value}")
                     if channel == 3:  # Pressure reading
@@ -126,10 +126,10 @@ class LoaderSystem:
                 await self.feeder_queue.put(1)  # Add to queue
             # Defeeder: If queue not empty, process next cell
             if not self.defeeder_queue.empty():
-                magazine_id = await self.defeeder_queue.get()  # Remove from queue
-                self.logger.info(f"Processing defeeder to magazine {magazine_id}...")
+                magazine = await self.defeeder_queue.get()  # Remove from queue
+                self.logger.info(f"Processing defeeder to magazine {magazine.name}...")
                 await asyncio.get_event_loop().run_in_executor(
-                    None, self.mag_distributor.defeeder_to_mag, magazine_id
+                    None, self.mag_distributor.defeeder_to_mag, magazine
                 )
             await asyncio.sleep(2)  # Adjust interval as needed
 

playgrounds/integration_test.py

@@ -3,7 +3,7 @@ import sys
 import asyncio
 import logging
 from robot_control.src.robot.controller import RobotController, Cell, CellStatus
-from robot_control.src.utils.config import ConfigParser
+from robot_control.src.utils.config import ConfigParser, DefeederMagazineConfig
 from robot_control.src.utils.logging import setup_logging
 from robot_control.src.vision.datamatrix import DataMatrixReader
 from robot_control.src.api.i2c_handler import I2C, MockI2C
@@ -50,8 +50,8 @@ class LoaderSystem:
         i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
         self.i2c = I2C(i2c_device_class)
 
-        self.feeder_queue: asyncio.Queue[int] = asyncio.Queue(self.config.feeder.max_capacity)
-        self.defeeder_queue: asyncio.Queue[int]  = asyncio.Queue(self.config.defeeder.max_capacity)
+        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)
 
         # Initialize robot controller with all required arguments
         self.controller = RobotController(

robot_control/config/config.yaml

@@ -33,42 +33,49 @@ feeder:
     pos_mm: 0
     rot_deg: 0
   min_voltage: 2.0
-  max_capacity: 10
+  max_num_cells: 10
 
 defeeder:
   robot_pos: [167.5, 630, 40]
   mag_pos: 
     pos_mm: 0
     rot_deg: -90
-  max_capacity: 10
+  max_num_cells: 10
 
 feeder_magazines:
   - mag_pos:
       pos_mm: 100
       rot_deg: 90
-    capacity: 70
+    max_num_cells: 70
   - mag_pos:
       pos_mm: 200
       rot_deg: 90
-    capacity: 70
+    max_num_cells: 70
   - mag_pos:
       pos_mm: 300
       rot_deg: 90
-    capacity: 70
+    max_num_cells: 70
 
+# Order of magazines is implying the priority
 defeeder_magazines:
+  - mag_pos:
+      pos_mm: 100
+      rot_deg: -90
+    max_num_cells: 70
+    health_range: [60, 100]
+    name: accept
   - mag_pos:
       pos_mm: 200
       rot_deg: -90
-    capacity: 70
-    health_threshold: 0.8
-    name: accepted
+    max_num_cells: 70
+    health_range: [0, 60]
+    name: reject
   - mag_pos:
-      pos_mm: 100
+      pos_mm: 300
       rot_deg: -90
-    capacity: 70
-    health_threshold: 0
-    name: rejected
+    max_num_cells: 70
+    health_range: [0, 100]
+    name: error
 
 mag_distributor:
     debug: True

robot_control/src/api/mqtt_handler.py

@@ -15,7 +15,7 @@ class MeasurementResult(BaseModel):
     device_id: str
     slot_id: int
     cell_id: int
-    capacity: float
+    health: float
     status: str
 
 class MQTTHandler:
@@ -35,7 +35,7 @@ class MQTTHandler:
         self.client.connect(broker, port, 60)
         self.client.loop_start()
 
-    def register_device(self, device_id, num_slots, callback: Callable = None):
+    def register_device(self, device_id, num_slots, callback: Callable):
         """Register a new device to handle"""
         device = MQTTDevice(device_id, num_slots)
         self.devices.append(device)
@@ -47,8 +47,7 @@ class MQTTHandler:
     def _subscribe_device_topics(self, device_id: str):
         """Subscribe to all topics for a specific device"""
         topics = [
-            f"measurement_done/{device_id}",
-            f"soa/{device_id}"
+            f"measurement_done/{device_id}", # add other topics as needed
         ]
         for topic in topics:
             self.client.subscribe(topic)
@@ -77,10 +76,6 @@ class MQTTHandler:
                     self.measurement_callbacks[device_id][result.slot_id](result)
                 else:
                     logger.warning(f"No callback for measurement {result}")
-                    
-            elif topic.startswith("soa/"):
-                logger.info(f"SOA update for device {device_id}: {payload}")
-                # TODO[SG]: Handle SOA update here
                 
         except Exception as e:
             logger.error(f"Error processing message: {e}")

robot_control/src/robot/controller.py

@@ -1,7 +1,7 @@
 from dataclasses import dataclass
 from enum import Enum
 from typing import List, Tuple, Optional
-from robot_control.src.utils.config import RobotConfig, SlotConfig, DeviceConfig, DropoffGradeConfig
+from robot_control.src.utils.config import RobotConfig, SlotConfig, DeviceConfig, DefeederMagazineConfig
 from robot_control.src.robot.movement import RobotMovement
 import logging
 from robot_control.src.api.mqtt_handler import MQTTHandler, MeasurementResult
@@ -25,10 +25,10 @@ class CellStatus(Enum):
 class Cell:
     id: int
     status: CellStatus = CellStatus.WAITING
-    capacity: float = 0
+    health: float = 0
 
 class RobotController:
-    def __init__(self, config: RobotConfig, gpio_handler: GPIOInterface, i2c, vision, pump_controller, feeder_queue: asyncio.Queue[int], defeeder_queue: asyncio.Queue[int]):
+    def __init__(self, config: RobotConfig, gpio_handler: GPIOInterface, i2c, vision, pump_controller, feeder_queue: asyncio.Queue[int], defeeder_queue: asyncio.Queue[DefeederMagazineConfig]):
         # Store configuration and hardware interfaces
         self.config = config
         self.cells: dict[int, Cell] = {}
@@ -189,10 +189,10 @@ class RobotController:
         await self.pick_cell_from_slot(waiting_slot)
 
         if not cell: # Cell not found, create new
-            cell = Cell(measurement_result.cell_id, cell_status, capacity=measurement_result.capacity)
+            cell = Cell(measurement_result.cell_id, cell_status, health=measurement_result.health)
             self.cells[measurement_result.cell_id] = cell
         else:
-            cell.capacity = measurement_result.capacity
+            cell.health = measurement_result.health
             cell.status = cell_status
 
         await self.sort_cell(cell)
@@ -355,16 +355,16 @@ class RobotController:
             del self.cells[cell.id] # Remove cell from our database TODO [SG]: Should we keep it for history?
 
         if cell.status is CellStatus.ERROR:
-            cell.capacity = 0 # will be dropped off in the lowest grade
-        for idx, mag in enumerate(self.defeeder_magazines):
-            if cell.capacity >= mag.health_threshold:
-                await self.dropoff_cell(idx)
+            cell.health = 0 # will be dropped off in the lowest grade
+        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.capacity}")
+        logger.error(f"No suitable magazine found for cell {cell.id} with capacity {cell.health}")
         return False
     
-    async def dropoff_cell(self, defeeder_mag_idx: Optional[int] = None):
+    async def dropoff_cell(self, defeeder_mag: Optional[DefeederMagazineConfig] = None):
         """
         Drop off a cell into the specified defeeder magazine.
         Adds the magazine index to the defeeder queue for async handling.
@@ -374,15 +374,11 @@ class RobotController:
             return
 
         # If no defeeddefeeder_mag_idx is given, use the last one in the list
-        if defeeder_mag_idx is None:
-            if not self.defeeder_magazines:
-                logger.error("No dropoff magazines configured")
-                return
-            defeeder_mag_idx = len(self.defeeder_magazines)-1
-        # Add the grade id to defeeder_queue when a cell is dropped off
+        if defeeder_mag is None:
+            defeeder_mag = self.defeeder_magazines[-1]
         if self.defeeder_queue is not None:
             try:
-                await self.defeeder_queue.put(defeeder_mag_idx)
+                await self.defeeder_queue.put(defeeder_mag)
             except Exception as e:
                 logger.warning(f"Failed to put to defeeder_queue: {e}")
 

robot_control/src/robot/mag_distributor.py

@@ -1,6 +1,6 @@
 import logging
 import asyncio
-from robot_control.src.utils.config import RobotConfig
+from robot_control.src.utils.config import RobotConfig, DefeederMagazineConfig
 from robot_control.src.api.gpio import GPIOInterface
 from typing import Optional
 
@@ -144,17 +144,11 @@ class MagDistributor:
                 continue
         self.logger.warning("No more available magazines to pick from. All attempts failed.")
 
-    def defeeder_to_mag(self, magazine_id: int):
+    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.
         """
-        # Check magazine_id validity
-        magazines = self.config.defeeder_magazines
-        if magazine_id < 0 or magazine_id >= len(magazines):
-            self.logger.error(f"Invalid magazine_id: {magazine_id}")
-            return
-
         # Move to defeeder position
         pos = self.config.defeeder.mag_pos
         self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
@@ -165,10 +159,9 @@ class MagDistributor:
             return
 
         # Move to target magazine position
-        pos = magazines[magazine_id].mag_pos
-        self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
+        self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, magazine.mag_pos.rot_deg)
 
         # Optionally check for successful placement (if sensor logic applies)
-        self.logger.info(f"Cell collected from defeeder and moved to magazine {magazine_id}.")
+        self.logger.info(f"Cell collected from defeeder and moved to magazine {magazine.name}.")
 
 

robot_control/src/utils/config.py

@@ -38,22 +38,31 @@ class MagDistPosition(BaseModel):
 
 class MagazineConfig(BaseModel):
     mag_pos: MagDistPosition
-    capacity: int = Field(default=10, ge=0)
+    max_num_cells: int = Field(default=10, ge=0)
 
 class DefeederMagazineConfig(MagazineConfig):
-    health_threshold: float = Field(ge=0.0, le=1.0)
     name: str
+    health_range: Tuple[int, int] = Field(default=(0, 100))
+
+    @field_validator('health_range')
+    @classmethod
+    def validate_health_range(cls, v):
+        min_val = 0
+        max_val = 100
+        if not (min_val <= v[0] < v[1] <= max_val):
+            raise ValueError(f"health_range must be a tuple (min, max) with {min_val} <= min < max <= {max_val}")
+        return v
 
 class FeederConfig(BaseModel):
     robot_pos: Tuple[float, float, float]
     mag_pos: MagDistPosition
     min_voltage: float = Field(default=2.0, ge=0.0)
-    max_capacity: int = Field(default=10, ge=0)
+    max_num_cells: int = Field(default=10, ge=0)
 
 class DefeederConfig(BaseModel):
     robot_pos: Tuple[float, float, float]
     mag_pos: MagDistPosition
-    max_capacity: int = Field(default=10, ge=0)
+    max_num_cells: int = Field(default=10, ge=0)
 
 class MagDistributorConfig(BaseModel):
     debug: Optional[bool] = False
@@ -61,12 +70,6 @@ class MagDistributorConfig(BaseModel):
     home_speed_mmmin: int = Field(default=50, ge=0)
     length_mm: float = Field(default=800.0, ge=0.0)
 
-class DropoffGradeConfig(BaseModel):
-    id: str
-    x_pos: float = Field(default=0.0, ge=0, le=700)
-    rot_deg: float = Field(default=0.0, ge=-180.0, le=180.0)
-    health_threshold: float = Field(ge=0.0, le=1.0)
-
 class MQTTConfig(BaseModel):
     broker: str = "localhost"
     port: int = Field(default=1883, ge=1, le=65535)
@@ -164,19 +167,13 @@ class RobotConfig(BaseModel):
     logging: LoggingConfig
 
     @model_validator(mode='after')
-    def validate_defeeder_magazines(self) -> 'RobotConfig':
-        # Sort grades by threshold in descending order for consistent behavior
-
-        sorted_defeeding_mags = sorted(self.defeeder_magazines, key=lambda x: x.health_threshold, reverse=True)
-        for i in range(1, len(sorted_defeeding_mags)):
-            if sorted_defeeding_mags[i-1].health_threshold <= sorted_defeeding_mags[i].health_threshold:
-                raise ValueError(
-                    f"Dropoff grade thresholds must be in strictly descending order. Found: "
-                    f"Grade {sorted_defeeding_mags[i-1].name} ({sorted_defeeding_mags[i-1].health_threshold}) <= "
-                    f"Grade {sorted_defeeding_mags[i].name} ({sorted_defeeding_mags[i].health_threshold})"
-                )
-        # Store sorted grades to ensure consistent behavior
-        self.defeeder_magazines = sorted_defeeding_mags
+    def check_lists(self) -> 'RobotConfig':
+        if not self.measurement_devices:
+            raise ValueError("measurement_devices list must not be empty")
+        if not self.feeder_magazines:
+            raise ValueError("feeder_magazines list must not be empty")
+        if not self.defeeder_magazines:
+            raise ValueError("defeeder_magazines list must not be empty")
         return self
 
 class ConfigParser:

tests/test_api.py

@@ -23,7 +23,8 @@ def mqtt_handler(mock_mqtt_client):
 
 class TestMQTTHandler:
     def test_device_registration(self, mqtt_handler:MQTTHandler):
-        mqtt_handler.register_device("test_device", 4)
+        callback = Mock()
+        mqtt_handler.register_device("test_device", 4, callback)
         assert "test_device" in mqtt_handler.measurement_callbacks
 
     def test_start_measurement(self, mqtt_handler:MQTTHandler, mock_mqtt_client:mqtt.Client):
@@ -46,7 +47,7 @@ class TestMQTTHandler:
             cell_id=123,
             device_id="test_device",
             slot_id=1,
-            capacity=3000.0,
+            health=100,
             status="complete"
         )
         

tests/test_robot.py

@@ -20,8 +20,8 @@ def robot_controller():
     config.grbl.port = "debug"
     config.mqtt.broker = "debug"  # connects to test mqtt broker
     mock_gpio = MockGPIO()
-    feeder_queue = asyncio.Queue(config.feeder.max_capacity)
-    defeeder_queue = asyncio.Queue(config.defeeder.max_capacity)
+    feeder_queue = asyncio.Queue(config.feeder.max_num_cells)
+    defeeder_queue = asyncio.Queue(config.defeeder.max_num_cells)
     controller = RobotController(
         config,
         gpio_handler=mock_gpio,
@@ -100,7 +100,7 @@ class TestRobotController:
         cell = Cell(
             id=1234,
             status=CellStatus.COMPLETED,
-            capacity=3000.0
+            health=100
         )
         robot_controller.gripper_occupied = False
         assert not await robot_controller.sort_cell(cell)
@@ -121,7 +121,7 @@ class TestRobotController:
         cell = Cell(
             id=0,
             status=CellStatus.COMPLETED,
-            capacity=3000.0
+            health=100
         )
         robot_controller.gripper_occupied = True
         await robot_controller.sort_cell(cell)
@@ -131,7 +131,7 @@ class TestRobotController:
         low_cap_cell = Cell(
             id=1234,
             status=CellStatus.COMPLETED,
-            capacity=0.0
+            health=0.0
         )
         robot_controller.gripper_occupied = True
         await robot_controller.sort_cell(low_cap_cell)