add I2C and pump controller integration; update configuration for vacuum system

main.py

@@ -3,6 +3,9 @@ from robot_control.src.utils.logging import LoggerSingleton
 from robot_control.src.robot.controller import RobotController
 from robot_control.src.utils.config import ConfigParser
 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.utils.pump_controller import PumpController
 
 class LoaderSystem:
     def __init__(self):
@@ -13,15 +16,40 @@ class LoaderSystem:
         self.vision = DataMatrixReader(cam)
         self.logger.info("Initializing LoaderSystem")
         self.vision.initialize()
+        self.i2c = I2C(MCP3428)
+        self.i2c.initialize()
+        self.logger.info("I2C initialized for MCP3428")
+
+        self.pump_controller = PumpController(self.config, self.logger)
 
     async def run(self):
         await self.controller.connect()
 
-        # Main robot control loop
+        await asyncio.gather(
+            self._loader_loop(),
+            self._poll_i2c_channels()
+        )
+
+    async def _poll_i2c_channels(self):
         while True:
-            await asyncio.sleep(0.1) # avoid busy loop
+            try:
+                readings = await self.i2c.read_channels([1, 3, 4])
+                for channel, value in readings.items():
+                    self.logger.info(f"Channel {channel} reading: {value}")
+                    if channel == 3:  # Pressure reading
+                        self.pump_controller.handle_pressure_reading(value)  # Delegate to PumpController
+                    # Perform a generic task based on the reading
+                    if value > 0.5:  # Example condition
+                        self.logger.warning(f"Value {value} on channel {channel} exceeds threshold")
+            except Exception as e:
+                self.logger.error(f"Error polling I2C channels: {str(e)}")
+            await asyncio.sleep(1)  # Poll every second
 
-            #Check for free slots loop
+    async def _loader_loop(self):
+        while True:
+            await asyncio.sleep(0.1)  # avoid busy loop
+
+            # Check for free slots loop
             while True:
                 slot = self.controller.get_next_free_slot()
                 if not slot:
@@ -39,10 +67,12 @@ class LoaderSystem:
                 except Exception as e:
                     self.logger.error(f"Failed to process cell {cell_id}: {str(e)}")
                     break
-            
+
             # Check for completed measurements and sort cell
             await self.controller.process_finished_measurement()
 
+    def __del__(self):
+        self.pump_controller.cleanup()  # Ensure PumpController cleans up gpio
 
 if __name__ == "__main__":
     loader_system = LoaderSystem()

robot_control/config/config.yaml

@@ -56,6 +56,13 @@ vision:
   exposure: 0.1
   gain: 1.0
 
+vacuum:
+  min_pressure_bar: 0.5
+  max_pressure_bar: 1.0
+  max_pump_time_s: 30
+  pump_gpio_pin: 17
+  valve_gpio_pin: 27
+
 system_settings:
   speed: 400.0
   acceleration: 20.0

robot_control/requirements.txt

@@ -10,4 +10,5 @@ pytest-asyncio==0.21.1
 pytest-cov==6.0.0
 httpx==0.25.0
 paho-mqtt<2
-pyserial-asyncio>=0.6
+pyserial-asyncio>=0.6
+pigpio

robot_control/src/utils/pump_controller.py

@@ -0,0 +1,51 @@
+import time
+import pigpio
+
+class PumpController:
+    def __init__(self, config, logger):
+        self.logger = logger
+        gpio_config = config.get_gpio_config()
+        self.pump_pin = gpio_config.get("pump_pin")
+        self.pressure_threshold_on = gpio_config.get("pressure_threshold_on")
+        self.pressure_threshold_off = gpio_config.get("pressure_threshold_off")
+        self.pump_watchdog_timeout = gpio_config.get("pump_watchdog_timeout")
+        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")
+
+    def handle_pressure_reading(self, value):
+        current_time = time.time()
+
+        # Check if pump needs to be deactivated due to watchdog timeout
+        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.pump_active = False
+            self.pump_last_activated = None
+            return
+
+        # 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.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.pump_active = False
+            self.pump_last_activated = None
+
+    def cleanup(self):
+        self.pi.write(self.pump_pin, 0)  # Ensure pump is off
+        self.pi.stop()  # Disconnect from pigpio daemon
+        self.logger.info("Pigpio cleanup completed")