feat: integrate MagDistributor for improved magazine handling; add feeder and defeeder queue management

main.py

@@ -8,6 +8,7 @@ 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.utils.logging import setup_logging
+from robot_control.src.robot.mag_distributor import MagDistributor
 
 class LoaderSystem:
     def __init__(self):
@@ -32,13 +33,21 @@ class LoaderSystem:
 
         self.pump_controller = PumpController(self.config, self.gpio)
 
+        # Initialize MagDistributor
+        self.mag_distributor = MagDistributor(self.config, self.gpio)
+
+        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)
+
         # Pass all hardware interfaces to the controller
         self.controller = RobotController(
             self.config,
             self.gpio,
             self.i2c,
             self.vision,
-            self.pump_controller
+            self.pump_controller,
+            self.feeder_queue,
+            self.defeeder_queue
         )
 
     async def run(self):
@@ -46,7 +55,8 @@ class LoaderSystem:
         try:
             await asyncio.gather(
                 self._loader_loop(),
-                self._poll_i2c_channels()
+                self._poll_i2c_channels(),
+                self._queue_monitor_loop(),
             )
         finally:
             self.cleanup()
@@ -84,6 +94,28 @@ class LoaderSystem:
             # Check for completed measurements and sort cell
             await self.controller.process_finished_measurement()
 
+    async def _queue_monitor_loop(self):
+        """
+        Periodically checks feeder and defeeder queues and calls placeholder functions.
+        """
+        while True:
+            # Feeder: If queue below max, trigger refill placeholder
+            if not self.feeder_queue.full():
+                self.logger.info(f"Refilling feeder...")
+                await asyncio.get_event_loop().run_in_executor(
+                    None, self.mag_distributor.mag_to_feeder
+                )
+                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}...")
+                await asyncio.get_event_loop().run_in_executor(
+                    None, self.mag_distributor.defeeder_to_mag, magazine_id
+                )
+            await asyncio.sleep(2)  # Adjust interval as needed
+
+
     def cleanup(self):
         self.gpio.cleanup()  # Ensure PumpController cleans up gpio
 

robot_control/config/config.yaml

@@ -30,13 +30,20 @@ measurement_devices:
 feeder:
   position: [27.5, 621.5, 47.5]
   min_voltage: 2.0
+  max_capacity: 10
+
+defeeder:
+  position: [167.5, 630, 40]
+  max_capacity: 10
 
 dropoff_grades:
   - id: accepted
-    position: [167.5, 630, 40]
+    x_pos: 200
+    rot_deg: -90
     capacity_threshold: 0.8
   - id: rejected
-    position: [167.5, 630, 40]
+    x_pos: 100
+    rot_deg: -90
     capacity_threshold: 0
 
 mqtt:
@@ -71,10 +78,19 @@ gpio:
   pump_pin: 17
   valve_pin: 27
   probe_pin: 22
+  
   measure_dir_pin: 11
   measure_step_pin: 9
   measure_en_pin: 10
 
+  dist_pos_dir_pin: 000
+  dist_pos_step_pin: 000
+  dist_pos_en_pin: 000
+
+  dist_rot_dir_pin: 000
+  dist_rot_step_pin: 000
+  dist_rot_en_pin: 000
+
 i2c:
   debug: false
 

robot_control/src/api/gpio.py

@@ -38,9 +38,15 @@ class PiGPIO(GPIOInterface):
             self.pi.set_mode(pin, pigpio.INPUT)
 
     def set_pin(self, pin: int, value: int) -> None:
+        if pin not in self.out_pins:
+            self.pi.set_mode(pin, pigpio.OUTPUT)
+            self.out_pins.append(pin)
         self.pi.write(pin, value)
 
     def get_pin(self, pin: int) -> int:
+        if pin not in self.in_pins:
+            self.pi.set_mode(pin, pigpio.INPUT)
+            self.in_pins.append(pin)
         return self.pi.read(pin)  
 
     def do_step(self, dir_pin: int, step_pin: int, steps: int = 100, step_delay_us: int = 200, direction: bool = True):
@@ -64,6 +70,8 @@ class PiGPIO(GPIOInterface):
 
             self.pi.wave_delete(wave_id)
 
+        self.set_pin(dir_pin, 0)
+
     def cleanup(self):
         if self.pi.connected:
             for pin in self.out_pins:

robot_control/src/robot/controller.py

@@ -27,7 +27,7 @@ class Cell:
     capacity: float = 0
 
 class RobotController:
-    def __init__(self, config: RobotConfig, gpio_handler: GPIOInterface, i2c, vision, pump_controller):
+    def __init__(self, config: RobotConfig, gpio_handler: GPIOInterface, i2c, vision, pump_controller, feeder_queue: asyncio.Queue[int], defeeder_queue: asyncio.Queue[int]):
         self.config = config
         self.cells: dict[int, Cell] = {}
         self.devices = self.config.measurement_devices
@@ -38,6 +38,8 @@ class RobotController:
         self.vision = vision
         self.pump_controller = pump_controller
         self.next_cell_id = ""
+        self.feeder_queue = feeder_queue
+        self.defeeder_queue = defeeder_queue
         
         # Initialize robot movement
         self.grbl_handler = GRBLHandler(
@@ -179,8 +181,13 @@ class RobotController:
         if self.gripper_occupied:
             logger.error("Gripper already occupied")
             return False
-            
-        feeder_pos = self.feeder.position
+        # Remove one item from feeder_queue when a cell is picked
+        if self.feeder_queue is not None:
+            try:
+                await self.feeder_queue.get()
+            except Exception as e:
+                logger.warning(f"Failed to get from feeder_queue: {e}")
+        feeder_pos = self.feeder.robot_pos
         feeder_pos_close = (feeder_pos[0], feeder_pos[1], feeder_pos[2]-2)
         safe_pos = (feeder_pos[0], feeder_pos[1], self.config.movement.safe_height)
         feeder_pos_side = (feeder_pos[0]-25, feeder_pos[1], feeder_pos[2])
@@ -330,8 +337,13 @@ class RobotController:
                 logger.error("No dropoff grades configured")
                 return
             dropoff_grade = self.dropoff_grades[-1]
-
-        pos = dropoff_grade.position
+        # Add the grade id to defeeder_queue when a cell is dropped off
+        if self.defeeder_queue is not None:
+            try:
+                await self.defeeder_queue.put(int(dropoff_grade.id))
+            except Exception as e:
+                logger.warning(f"Failed to put to defeeder_queue: {e}")
+        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}...")
         await self.movement.move_to_position(*safe_pos)

robot_control/src/robot/mag_distributor.py

@@ -0,0 +1,127 @@
+import logging
+import asyncio
+from robot_control.src.utils.config import RobotConfig
+from robot_control.src.api.gpio import GPIOInterface
+from typing import Optional
+
+
+class MagDistributor:
+    def __init__(self, config: RobotConfig, gpio: GPIOInterface):
+        self.config = config
+        self.gpio = gpio
+
+        # Pin assignments from config
+        gpio_conf = config.gpio
+        self.linear_dir_pin = gpio_conf.mag_dist_pos_dir_pin
+        self.linear_step_pin = gpio_conf.mag_dist_pos_step_pin
+        # self.linear_en_pin = gpio_conf.mag_dist_pos_en_pin
+        self.rot_dir_pin = gpio_conf.mag_dist_rot_dir_pin
+        self.rot_step_pin = gpio_conf.mag_dist_rot_step_pin
+        # self.rot_en_pin = gpio_conf.mag_dist_rot_en_pin
+
+        # Endstop pins
+        self.linear_limit_pin = gpio_conf.mag_dist_pos_limit_pin
+        self.rot_limit_pin = gpio_conf.mag_dist_rot_limit_pin
+
+        # Cell pick sensor pin
+        self.mag_dist_sensor_pin = gpio_conf.mag_dist_sensor_pin  # <-- Add this to your config
+
+        # Max travel (mm or steps, as appropriate)
+        self.full_rot_steps = 3200  # Assuming 3200 steps for a full rotation
+        self.linear_length_mm = config.mag_distributor.length_mm
+        self.steps_per_mm = self.full_rot_steps / 40 # Assuming 40mm per rotation
+        self.max_speed = config.mag_distributor.max_speed  # mm/s
+        self.home_speed = config.mag_distributor.home_speed  # mm/s
+
+        # Current position tracking (steps or mm/deg)
+        self.curr_pos_mm = 0
+        self.curr_rot_deg = 0
+
+        # Track current feeding magazine
+        self.current_magazine = 0
+
+        self.logger = logging.getLogger(__name__)
+
+    def _speed_to_step_delay(self, speed_mm_s):
+        """Convert speed in mm/s to step delay in ms for do_step."""
+        steps_per_s = speed_mm_s * self.steps_per_mm
+        if steps_per_s == 0:
+            return 1000  # fallback to slow
+        step_delay_s = 1.0 / steps_per_s
+        return int(step_delay_s * 1000)  # ms
+
+    async def home(self):
+        """Home both axes using endstops."""
+        self.logger.info("Homing linear axis...")
+        max_linear_steps = int(self.linear_length_mm * self.steps_per_mm)
+        home_step_delay = self._speed_to_step_delay(self.home_speed)
+        await self._home_axis(self.linear_dir_pin, self.linear_step_pin, self.linear_limit_pin, axis='linear', max_steps=max_linear_steps, step_delay=home_step_delay)
+        self.curr_pos_mm = 0
+        self.logger.info("Homing rotational axis...")
+        await self._home_axis(self.rot_dir_pin, self.rot_step_pin, self.rot_limit_pin, axis='rot', max_steps=self.full_rot_steps, step_delay=home_step_delay)
+        self.curr_rot_deg = 0
+        self.logger.info("Mag distributor homed successfully.")
+
+    async def _home_axis(self, dir_pin, step_pin, endstop_pin, axis='linear', max_steps = 10000, step_delay=200):
+        # Move in negative direction until endstop is triggered
+        for _ in range(max_steps):
+            if self.gpio.get_pin(endstop_pin):
+                break
+            self.gpio.do_step(dir_pin, step_pin, 1, step_delay, direction=True)
+            await asyncio.sleep(0.001)
+        else:
+            raise RuntimeError(f"{axis} axis homing failed")
+        self.logger.info(f"{axis} axis homed.")
+
+    def move_mag_distributor_at_pos(self, pos_target, rot_target, step_delay=None):
+        self.logger.info(f"Moving mag distributor to linear: {pos_target} mm, rot: {rot_target} deg")
+        linear_steps = int(abs(pos_target - self.curr_pos_mm) * self.steps_per_mm)
+        rot_steps = int(abs(rot_target - self.curr_rot_deg) / 360 * self.full_rot_steps)
+        linear_dir = True if pos_target > self.curr_pos_mm else False
+        rot_dir = True if rot_target > self.curr_rot_deg else False
+        # Use max speed for normal moves if not specified
+        if step_delay is None:
+            step_delay = self._speed_to_step_delay(self.max_speed)
+        if linear_steps > 0:
+            self.gpio.do_step(self.linear_dir_pin, self.linear_step_pin, linear_steps, step_delay, linear_dir)
+            self.curr_pos_mm = pos_target
+        if rot_steps > 0:
+            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 mag_to_feeder(self, magazine_id: Optional[int] = None):
+        """
+        Move a cell from a magazine to the feeder.
+        If magazine_id is None, use current_magazine and try next if pick fails.
+        """
+        magazines = self.config.feeder.magazines
+        num_magazines = len(magazines)
+        tried = 0
+        if magazine_id is not None:
+            mags_to_try = [magazine_id]
+        else:
+            mags_to_try = list(range(self.current_magazine, num_magazines)) + list(range(0, self.current_magazine))
+        for mag_id in mags_to_try:
+            pos = magazines[mag_id].pos
+            self.move_mag_distributor_at_pos(pos[0], pos[1])
+            # Check cell pick sensor
+            if self.gpio.get_pin(self.mag_dist_sensor_pin):
+                pos = self.config.feeder.mag_pos
+                self.move_mag_distributor_at_pos(pos[0], pos[1])
+                self.logger.info(f"Cell successfully picked from magazine {mag_id} and deposited to feeder.")
+                self.current_magazine = mag_id  # update current
+                return
+            else:
+                self.logger.warning(f"Failed to pick cell from magazine {mag_id}. Trying next magazine.")
+                tried += 1
+                continue
+        self.logger.warning("No more available magazines to pick from. All attempts failed.")
+
+    def defeeder_to_mag(self, magazine_id: int):
+        pos = self.config.defeeder.mag_pos
+        self.move_mag_distributor_at_pos(pos[0], pos[1])
+        pos = self.config.defeeder.magazines[magazine_id].pos
+        self.move_mag_distributor_at_pos(pos[0], pos[1])
+        self.logger.info("Cell collected from defeeder.")
+
+

robot_control/src/utils/config.py

@@ -26,13 +26,31 @@ class DeviceConfig(BaseModel):
             raise ValueError("Duplicate slot IDs found")
         return v
 
+class MagazineConfig(BaseModel):
+    pos : Tuple[float, float]
+    capacity: int = Field(default=10, ge=0)
 class FeederConfig(BaseModel):
-    position: Tuple[float, float, float]
+    robot_pos: Tuple[float, float, float]
+    mag_pos : Tuple[float, float]
     min_voltage: float = Field(default=2.0, ge=0.0)
+    max_capacity: int = Field(default=10, ge=0)
+    magazines: List[MagazineConfig] = Field(default_factory=list)
+
+class DefeederConfig(BaseModel):
+    robot_pos: Tuple[float, float, float]
+    mag_pos : Tuple[float, float]
+    max_capacity: int = Field(default=10, ge=0)
+    magazines: List[MagazineConfig] = Field(default_factory=list)
+
+class MagDistributorConfig(BaseModel):
+    max_speed: float = Field(default=100.0, ge=0.0)
+    home_speed: float = Field(default=50.0, ge=0.0)
+    length_mm: float = Field(default=800.0, ge=0.0)
 
 class DropoffGradeConfig(BaseModel):
     id: str
-    position: Tuple[float, float, float]
+    x_pos: float = Field(default=0.0, ge=0, le=700)
+    rot_deg: float = Field(default=0.0, ge=-180.0, le=180.0)
     capacity_threshold: float = Field(ge=0.0, le=1.0)
 
 class MQTTConfig(BaseModel):
@@ -77,12 +95,21 @@ class VacuumConfig(BaseModel):
 
 class GPIOConfig(BaseModel):
     debug: bool = False
-    pump_pin: int = Field(default=17, ge=0, le=25)
-    valve_pin: int = Field(default=27, ge=0, le=25)
-    probe_pin: int = Field(default=22, ge=0, le=25)
-    measure_dir_pin: int = Field(default=11, ge=0, le=25)
-    measure_step_pin: int = Field(default=9, ge=0, le=25)
-    measure_en_pin: int = Field(default=10, ge=0, le=25)
+    pump_pin: int = Field(ge=0, le=25)
+    valve_pin: int = Field(ge=0, le=25)
+    probe_pin: int = Field(ge=0, le=25)
+    measure_dir_pin: int = Field(ge=0, le=25)
+    measure_step_pin: int = Field(ge=0, le=25)
+    measure_en_pin: int = Field(default=0, ge=0, le=25)
+    mag_dist_pos_dir_pin: int = Field(ge=0, le=25)
+    mag_dist_pos_step_pin: int = Field(ge=0, le=25)
+    mag_dist_pos_en_pin: int = Field(default=0, ge=0, le=25)
+    mag_dist_pos_limit_pin: int = Field(ge=0, le=25)
+    mag_dist_rot_dir_pin: int = Field(ge=0, le=25)
+    mag_dist_rot_step_pin: int = Field(ge=0, le=25)
+    mag_dist_rot_en_pin: int = Field(default=0, ge=0, le=25)
+    mag_dist_rot_limit_pin: int = Field(ge=0, le=25)
+    mag_dist_sensor_pin: int = Field(ge=0, le=25)
 
 class I2CConfig(BaseModel):
     debug: bool = False
@@ -110,6 +137,8 @@ class LoggingConfig(BaseModel):
 class RobotConfig(BaseModel):
     measurement_devices: List[DeviceConfig]
     feeder: FeederConfig
+    defeeder: DefeederConfig
+    mag_distributor: MagDistributorConfig
     dropoff_grades: List[DropoffGradeConfig]
     mqtt: MQTTConfig
     grbl: GRBLConfig