refactor: migrate pump and valve GPIO configuration to a dedicated file; abstract usage

main.py

@@ -6,12 +6,17 @@ from robot_control.src.vision.datamatrix import DataMatrixReader
 from robot_control.src.api.i2c_handler import I2C
 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
 
 class LoaderSystem:
     def __init__(self):
+        pump_pin = self.config.get_gpio_config().get("pump_pin")
+        valve_pin = self.config.get_gpio_config().get("valve_pin")
+        self.gpio = PiGPIO([pump_pin, valve_pin], [])
+
         self.config = ConfigParser()
         self.logger = LoggerSingleton.get_logger(self.config)
-        self.controller = RobotController(self.config)
+        self.controller = RobotController(self.config, self.gpio)
         cam = self.config.get_vision_config().get("camera_id")
         self.vision = DataMatrixReader(cam)
         self.logger.info("Initializing LoaderSystem")
@@ -20,7 +25,8 @@ class LoaderSystem:
         self.i2c.initialize()
         self.logger.info("I2C initialized for MCP3428")
 
-        self.pump_controller = PumpController(self.config, self.logger)
+
+        self.pump_controller = PumpController(self.config, self.gpio, self.logger)
 
     async def run(self):
         await self.controller.connect()

robot_control/config/config.yaml

@@ -60,11 +60,14 @@ vacuum:
   min_pressure_bar: 0.5
   max_pressure_bar: 1.0
   max_pump_time_s: 30
-  pump_gpio_pin: 17
-  valve_gpio_pin: 27
   gripping_threshold_v: 0.8
   pump_watchdog_timeout_s: 30
 
+gpio:
+  debug: false
+  pump_pin: 17
+  valve_pin: 27
+
 system_settings:
   speed: 400.0
   acceleration: 20.0

robot_control/src/api/gpio.py

@@ -0,0 +1,40 @@
+from abc import ABC, abstractmethod
+import pigpio
+
+class GPIOInterface(ABC):
+    def __init__(self, in_pins: list=[], out_pins: list=[]):
+        self.out_pins = out_pins
+        self.in_pins = in_pins
+
+    @abstractmethod
+    def set_pin(self, pin: int, value: int) -> None:
+        pass
+    
+    @abstractmethod
+    def get_pin(self, pin: int) -> int:
+        pass
+
+class PiGPIO(GPIOInterface):
+    def __init__(self, in_pins: list=[], out_pins: list=[]):
+        super().__init__(in_pins, out_pins)
+        self.pi = pigpio.pi()
+        if not self.pi.connected:
+            raise RuntimeError("Could not connect to pigpiod. Is it running? Try: 'sudo systemctl start pigpiod'")
+        for pin in out_pins:
+            self.pi.set_mode(pin, pigpio.OUTPUT)
+            self.pi.write(pin, 0)  # Ensure output pins are off initially
+        for pin in in_pins:
+            self.pi.set_mode(pin, pigpio.INPUT)
+
+    def set_pin(self, pin: int, value: int) -> None:
+        self.pi.write(pin, value)
+
+    def get_pin(self, pin: int) -> int:
+        return self.pi.read(pin)
+
+class MockGPIO(GPIOInterface):
+    def set_pin(self, pin: int, value: int) -> None:
+        pass
+
+    def get_pin(self, pin: int) -> int:      
+        return 0 

robot_control/src/robot/controller.py

@@ -6,6 +6,7 @@ from robot_control.src.robot.movement import RobotMovement
 from robot_control.src.utils.logging import LoggerSingleton
 from robot_control.src.api.mqtt_handler import MQTTHandler, MeasurementResult
 from robot_control.src.api.grbl_handler import GRBLHandler
+from robot_control.src.api.gpio import GPIOInterface
 
 logger = LoggerSingleton.get_logger()
 
@@ -19,7 +20,6 @@ class CellStatus(Enum):
 class Cell:
     id: int
     status: CellStatus = CellStatus.WAITING
-    measurement_slot: int = None
     capacity: float = None
 
 class DropoffGrade:
@@ -28,7 +28,7 @@ class DropoffGrade:
         self.position = (x, y, z)
 
 class RobotController:
-    def __init__(self, config: ConfigParser):
+    def __init__(self, config: ConfigParser, gpio_handler: GPIOInterface):
         self.config = config
         self.cells:dict[Cell] = {}
         self.devices = self.config.get_devices()
@@ -38,11 +38,12 @@ class RobotController:
         self.system_settings = self.config.get_system_settings()
         
         # Initialize robot movement
-
         grbl_config = self.config.get_grbl_config()
-
         self.grbl_handler = GRBLHandler(grbl_config.get('port'), grbl_config.get('baudrate'))
-        self.movement = RobotMovement(self.grbl_handler)
+        self.movement = RobotMovement(self.config, self.grbl_handler)
+
+        self.gpio = gpio_handler
+        self.valve_pin = config.get_gpio_config().get("valve_pin")
         
         # Initialize with configured values
         self.total_slots = sum(len(device.slots) for device in self.devices)
@@ -67,6 +68,26 @@ class RobotController:
                 num_slots=len(device.slots),
                 callback=lambda measurement_result: self.work_queue.append(measurement_result)
             )
+
+    def activate_endeffector(self) -> bool:
+        """Activate the pump end effector via GPIO pin."""
+        try:
+            self.gpio.set_pin(self.valve_pin, 1)
+            self.suction_state = True
+            logger.debug("End effector activated")
+            return True
+        except Exception as e:
+            raise RuntimeError(f"Failed to activate end effector: {str(e)}")                
+
+    def deactivate_endeffector(self) -> bool:
+        """Deactivate the pump end effector via GPIO pin."""
+        try:
+            self.gpio.set_pin(self.valve_pin, 0)
+            self.suction_state = False
+            logger.debug("End effector deactivated")
+            return True
+        except Exception as e:
+            raise RuntimeError(f"Failed to deactivate end effector: {str(e)}")
     
     def set_suction_state(self, state:bool):
         self.suction_state = state
@@ -156,7 +177,7 @@ class RobotController:
 
         # Grip cell
         try:
-            await self.movement.activate_endeffector()
+            self.activate_endeffector()
             if not self.suction_state:
                 logger.error("Suction state is off, cannot pick cell")
                 return False
@@ -168,7 +189,7 @@ class RobotController:
         
         except Exception as e:
             logger.error(f"Failed to pick cell from feeder: {str(e)}")
-            await self.movement.deactivate_endeffector() # TODO [SG]: Should deactivate here?
+            self.deactivate_endeffector()
             self.gripper_occupied = False
             raise e
 
@@ -200,7 +221,7 @@ class RobotController:
         await self.movement.move_to_position(*pos)
 
         # Release cell
-        await self.movement.deactivate_endeffector() 
+        self.deactivate_endeffector() 
             
         slot.occupied = True
         slot.cell_id = cell.id
@@ -208,7 +229,6 @@ class RobotController:
 
         # Start measurement
         cell.status = CellStatus.MEASURING
-        # cell.measurement_slot = slot.slot_id # TODO[SG]: Make this consistent
         self.mqtt_handler.start_measurement(
             device_id=slot.device_id,
             slot=slot.slot_id,
@@ -238,7 +258,7 @@ class RobotController:
 
         # Grip cell
         try:
-            await self.movement.activate_endeffector()
+            self.activate_endeffector()
             if not self.suction_state:
                 logger.error("Suction state is off, cannot pick cell")
                 return False
@@ -253,7 +273,7 @@ class RobotController:
             logger.info(f"Cell {cell_id} collected from {slot}")
             return cell_id
         except Exception as e:
-            await self.movement.deactivate_endeffector() # Try to deactivate to avoid stuck gripper
+            self.deactivate_endeffector() # Try to deactivate to avoid stuck gripper
             self.gripper_occupied = False
             raise e
 
@@ -289,7 +309,7 @@ class RobotController:
         logger.info(f"Moving to dropoff position {pos}...")
         await self.movement.move_to_position(*pos)
         # Release cell
-        if not await self.movement.deactivate_endeffector():
+        if not self.deactivate_endeffector():
             raise RuntimeError("Failed to release cell")
         self.gripper_occupied = False
 

robot_control/src/robot/movement.py

@@ -10,17 +10,22 @@ from robot_control.src.utils.config import ConfigParser
 logger = LoggerSingleton.get_logger()
 
 class RobotMovement:
-    def __init__(self, grbl: GRBLHandler, config: ConfigParser):
-        self.pi = pigpio.pi()
+    """Class for controlling robot movements and end effector operations."""
+
+    def __init__(self, config: ConfigParser, grbl: GRBLHandler):
+        """Initialize the robot movement controller.
+
+        Args:
+            grbl (GRBLHandler): GRBL communication handler
+            config (ConfigParser): Configuration parser instance
+        """
         self.config = config        
-        vacuum_config = config.get_vacuum_config()
-        self.valve_gpio_pin = vacuum_config.get("valve_gpio_pin")
-        self.pi.set_mode(self.valve_gpio_pin, pigpio.OUTPUT)
-        self.pi.write(self.valve_gpio_pin, 0)  # Ensure pump is off initially
 
+        # Setting configs
         sys_config = config.get_system_settings()
         self.speed: float = sys_config.speed
 
+        # Basic variables for robot movement
         self.home_position: tuple[float, float, float] = (0.0, 0.0, 0.0)
         self.current_position: tuple[float, float, float] = (0.0, 0.0, 0.0)
         self.is_moving: bool = False
@@ -31,9 +36,22 @@ class RobotMovement:
         self.grbl.register_position_callback(self._position_callback)
 
     def _position_callback(self, pos: dict):
+        """Update the current position from GRBL feedback.
+
+        Args:
+            pos (dict): Position dictionary containing coordinates
+        """
         self.current_position = pos
         
     async def set_speed(self, speed_mms: float) -> None:
+        """Set the movement speed of the robot.
+
+        Args:
+            speed_mms (float): Speed in millimeters per second
+
+        Raises:
+            ValueError: If speed is not positive
+        """
         if speed_mms <= 0:
             raise ValueError("Speed must be positive")
         self.speed = speed_mms
@@ -41,6 +59,19 @@ class RobotMovement:
         await self.grbl.send_gcode([f'F{speed_mms * 60}'])  # Convert mm/s to mm/min
 
     async def move_to_position(self, x: float, y: float, z: float) -> tuple[float, float, float]:
+        """Move the robot to an absolute position.
+
+        Args:
+            x (float): X coordinate in mm
+            y (float): Y coordinate in mm
+            z (float): Z coordinate in mm
+
+        Returns:
+            tuple[float, float, float]: Actual position reached
+
+        Raises:
+            RuntimeError: If movement fails or times out
+        """
         try:
             self.is_moving = True
             logger.debug(f"Moving to position X:{x:.3f} Y:{y:.3f} Z:{z:.3f}")
@@ -60,13 +91,31 @@ class RobotMovement:
             self.is_moving = False
 
     async def move_relative(self, dx: float, dy: float, dz: float) -> tuple[float, float, float]:
+        """Move the robot by a relative distance.
+
+        Args:
+            dx (float): Relative X movement in mm
+            dy (float): Relative Y movement in mm
+            dz (float): Relative Z movement in mm
+
+        Returns:
+            tuple[float, float, float]: Actual position reached
+        """
         logger.debug(f"Moving relative by dX:{dx:.3f} dY:{dy:.3f} dZ:{dz:.3f}")
         await self.grbl.send_gcode(['G91'])  # Relative positioning
         result = await self.move_to_position(dx, dy, dz)
         await self.grbl.send_gcode(['G90'])  # Back to absolute
         return result
 
-    async def home(self) -> bool:
+    async def perform_homing(self) -> bool:
+        """Home all axes of the machine.
+
+        Returns:
+            bool: True if homing successful
+
+        Raises:
+            RuntimeError: If homing fails
+        """
         self.is_moving = True
         try:
             logger.info("Homing machine...")
@@ -78,36 +127,16 @@ class RobotMovement:
             raise RuntimeError(f"Homing failed: {str(e)}")
 
     def get_position(self) -> tuple[float, float, float]:
-        return self.current_position
-    
-    async def activate_endeffector(self) -> bool:
-        """Activate the pump end effector."""
-        try:
-            self.pi.write(self.valve_gpio_pin, 1)
-            self.endeffector_active = True
-            logger.debug("End effector activated")
-            return True
-        except Exception as e:
-            raise RuntimeError(f"Failed to activate end effector: {str(e)}")
-        
+        """Get the current position of the robot.
 
-    async def deactivate_endeffector(self) -> bool:
-        """Deactivate the pump end effector."""
-        try:
-            self.pi.write(self.valve_gpio_pin, 0)
-            self.endeffector_active = False
-            logger.debug("End effector deactivated")
-            return True
-        except Exception as e:
-            raise RuntimeError(f"Failed to deactivate end effector: {str(e)}")
-
-    def is_endeffector_active(self) -> bool:
-        """Get end effector state."""
-        return self.endeffector_active
+        Returns:
+            tuple[float, float, float]: Current (X, Y, Z) position
+        """
+        return self.current_position
 
     async def stop(self) -> None:
         """Emergency stop."""
         self.is_moving = False
         self.endeffector_active = False
         logger.warning("Emergency stop triggered!")
-        await self.grbl.send_gcode(['!', 'M5'])  # Feed hold + stop pump
+        await self.grbl.send_gcode(['!'])  # Feed hold

robot_control/src/robot/pump_controller.py

@@ -1,25 +1,22 @@
 import time
 import pigpio
 from robot_control.src.utils.config import ConfigParser
+from robot_control.src.api.gpio import GPIOInterface
 
 class PumpController:
-    def __init__(self, config:ConfigParser, logger):
+    def __init__(self, config:ConfigParser, gpio:GPIOInterface, logger):
         self.logger = logger
         vacuum_config = config.get_vacuum_config()
-        self.pump_pin = vacuum_config.get("pump_gpio_pin")
         self.pressure_threshold_on = vacuum_config.get("min_pressure_bar")
         self.pressure_threshold_off = vacuum_config.get("max_pressure_bar")
         self.pump_watchdog_timeout = vacuum_config.get("pump_watchdog_timeout", 30)  # Default to 30 seconds
         self.gripping_threshold = vacuum_config.get("gripping_threshold_v")
+
+        gpio_config = config.get_gpio_config()
+        self.pump_pin = gpio_config.get("pump_pin")
         self.pump_last_activated = None
         self.pump_active = False
-
-        self.pi = pigpio.pi()  # Initialize pigpio
-        if not self.pi.connected:
-            raise RuntimeError("Failed to connect to pigpio daemon")
-        self.pi.set_mode(self.pump_pin, pigpio.OUTPUT)
-        self.pi.write(self.pump_pin, 0)  # Ensure pump is off initially
-        self.logger.info(f"Pump GPIO pin {self.pump_pin} initialized using pigpio")
+        self.gpio = gpio
 
     def check_endeffector_state(self, value) -> bool:
         return value < self.gripping_threshold        
@@ -31,7 +28,7 @@ class PumpController:
         if self.pump_active and self.pump_last_activated and \
            (current_time - self.pump_last_activated > self.pump_watchdog_timeout):
             self.logger.warning("Pump deactivated due to watchdog timeout")
-            self.pi.write(self.pump_pin, 0)  # Turn off pump
+            self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
             self.pump_last_activated = None
             return
@@ -39,14 +36,14 @@ class PumpController:
         # Activate pump if pressure is above the threshold (lower value is more vacuum)
         if not self.pump_active and value > self.pressure_threshold_on:
             self.logger.info("Activating pump due to pressure threshold")
-            self.pi.write(self.pump_pin, 1)  # Turn on pump
+            self.gpio.set_pin(self.pump_pin, 1)  # Turn on pump
             self.pump_active = True
             self.pump_last_activated = current_time
 
         # Deactivate pump if pressure is below the threshold
         elif self.pump_active and value < self.pressure_threshold_off:
             self.logger.info("Deactivating pump due to pressure threshold")
-            self.pi.write(self.pump_pin, 0)  # Turn off pump
+            self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
             self.pump_last_activated = None
 

robot_control/src/utils/config.py

@@ -128,10 +128,16 @@ class ConfigParser:
             'min_pressure_bar': vacuum_config.get('min_pressure_bar', 0),
             'max_pressure_bar': vacuum_config.get('max_pressure_bar', 0),
             'max_pump_time_s': vacuum_config.get('max_pump_time_s', 30),
-            'pump_gpio_pin': vacuum_config.get('pump_gpio_pin', 17),
-            'valve_gpio_pin': vacuum_config.get('valve_gpio_pin', 27),
             'gripping_threshold_v': vacuum_config.get('gripping_threshold_v', 0.8)
         }
+    
+    def get_gpio_config(self) -> dict:
+        """Get gpio system configuration"""
+        gpio_config = self.config.get('gpio', {})
+        return {
+            'pump_pin': gpio_config.get('pump_pin', 17),
+            'valve_pin': gpio_config.get('valve_pin', 27)
+        }
 
     def get_logging_config(self) -> dict:
         """Get logging configuration"""

tests/test_robot.py

@@ -2,22 +2,22 @@ import pytest
 from robot_control.src.robot.movement import RobotMovement
 from robot_control.src.robot.controller import RobotController, Cell, CellStatus
 from robot_control.src.utils.config import ConfigParser
-
 from robot_control.src.api.grbl_handler import GRBLHandler
-from robot_control.src.utils.config import ConfigParser
+from robot_control.src.api.gpio import MockGPIO
 
 @pytest.fixture
 def robot_movement():
-    config_parser = ConfigParser()
-    grbl_config = config_parser.get_grbl_config()
+    config = ConfigParser()
+    grbl_config = config.get_grbl_config()
     grbl_handler = GRBLHandler(grbl_config.get('port'), grbl_config.get('baudrate'))
-    return RobotMovement(grbl_handler)
+    return RobotMovement(config, grbl_handler)
 
 @pytest.fixture
 def robot_controller():
-    config_parser = ConfigParser()
-    config_parser.config["mqtt"]["broker"] = "debug" # connects to test mqtt broker
-    controller = RobotController(config_parser)
+    config = ConfigParser()
+    config.config["mqtt"]["broker"] = "debug" # connects to test mqtt broker
+    mock_gpio = MockGPIO()
+    controller = RobotController(config, gpio_handler=mock_gpio)
     return controller
 
 class TestRobotMovement:
@@ -85,7 +85,6 @@ class TestRobotController:
         cell = Cell(
             id="test_cell",
             status=CellStatus.COMPLETED,
-            measurement_slot=1,
             capacity=3000.0
         )
         robot_controller.gripper_occupied = False
@@ -95,8 +94,7 @@ class TestRobotController:
     async def test_sort_failed_cell(self, robot_controller: RobotController):
         failed_cell = Cell(
             id=123,
-            status=CellStatus.ERROR,
-            measurement_slot=1
+            status=CellStatus.ERROR
         )
         robot_controller.gripper_occupied = True
         await robot_controller.sort_cell(failed_cell)
@@ -108,7 +106,6 @@ class TestRobotController:
         cell = Cell(
             id="test_cell",
             status=CellStatus.COMPLETED,
-            measurement_slot=1,
             capacity=3000.0
         )
         robot_controller.gripper_occupied = True
@@ -119,7 +116,6 @@ class TestRobotController:
         low_cap_cell = Cell(
             id="low_cap_cell",
             status=CellStatus.COMPLETED,
-            measurement_slot=1,
             capacity=0.0
         )
         robot_controller.gripper_occupied = True