ref: update pump controller logic; enhance configuration for vacuum system

.gitignore

@@ -1,7 +1,7 @@
 .cpython-39.pyc
 .VSCodeCounter/
 .pyc
-robot-control/src/robot_control.egg-info/
+robot_control/src/robot_control.egg-info
 __pycache__
 .coverage
 .python-version

main.py

@@ -5,7 +5,7 @@ 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
+from robot_control.src.robot.pump_controller import PumpController
 
 class LoaderSystem:
     def __init__(self):
@@ -37,7 +37,10 @@ class LoaderSystem:
                 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
+                        self.pump_controller.handle_tank_reading(value)
+                    if channel == 4:
+                        state = self.pump_controller.check_endeffector_state(value)
+                        self.controller.set_suction_state(state)
                     # 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")

playgrounds/gpio_playground.py

@@ -0,0 +1,30 @@
+import asyncio
+import pigpio
+
+async def toggle_pin(pin: int):
+    """Toggle a GPIO pin indefinitely."""
+    pi = pigpio.pi()
+    if not pi.connected:
+        raise RuntimeError(f"Failed to connect to pigpio daemon for pin {pin}")
+    
+    state = 0
+    try:
+        while True:
+            state = 1 - state  # Toggle state between 0 and 1
+            pi.write(pin, state)
+            await asyncio.sleep(1)  # Wait 1 second between toggles
+    finally:
+        pi.write(pin, 0)  # Ensure the pin is turned off
+        pi.stop()
+
+async def main():
+    """Run multiple toggle_pin tasks in parallel."""
+    pins = [17, 27, 22]  # Replace with the GPIO pins you want to toggle
+    tasks = [toggle_pin(pin) for pin in pins]
+    await asyncio.gather(*tasks)
+
+if __name__ == "__main__":
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print("Exiting...")

robot_control/config/config.yaml

@@ -62,6 +62,8 @@ vacuum:
   max_pump_time_s: 30
   pump_gpio_pin: 17
   valve_gpio_pin: 27
+  gripping_threshold_v: 0.8
+  pump_watchdog_timeout_s: 30
 
 system_settings:
   speed: 400.0

robot_control/src/robot/controller.py

@@ -49,6 +49,7 @@ class RobotController:
         self.work_queue: List[MeasurementResult] = []
 
         self.gripper_occupied = False
+        self.suction_state = False
         
         # Initialize MQTT handler
         mqtt_config = self.config.get_mqtt_config()
@@ -66,6 +67,9 @@ class RobotController:
                 num_slots=len(device.slots),
                 callback=lambda measurement_result: self.work_queue.append(measurement_result)
             )
+    
+    def set_suction_state(self, state:bool):
+        self.suction_state = state
 
     async def connect(self):
         await self.grbl_handler.connect()
@@ -153,6 +157,9 @@ class RobotController:
         # Grip cell
         try:
             await self.movement.activate_endeffector()
+            if not self.suction_state:
+                logger.error("Suction state is off, cannot pick cell")
+                return False
             self.gripper_occupied = True
             logger.info(f"Moving to feeder position (safe) {safe_pos}...")
             await self.movement.move_to_position(*safe_pos)
@@ -232,6 +239,9 @@ class RobotController:
         # Grip cell
         try:
             await self.movement.activate_endeffector()
+            if not self.suction_state:
+                logger.error("Suction state is off, cannot pick cell")
+                return False
             self.gripper_occupied = True
             cell_id = slot.cell_id
             slot.occupied = False

robot_control/src/utils/pump_controller.py → robot_control/src/robot/pump_controller.py

@@ -1,14 +1,16 @@
 import time
 import pigpio
+from robot_control.src.utils.config import ConfigParser
 
 class PumpController:
-    def __init__(self, config, logger):
+    def __init__(self, config:ConfigParser, 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")
+        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")
         self.pump_last_activated = None
         self.pump_active = False
 
@@ -19,7 +21,10 @@ class PumpController:
         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):
+    def check_endeffector_state(self, value) -> bool:
+        return value < self.gripping_threshold        
+
+    def handle_tank_reading(self, value):
         current_time = time.time()
 
         # Check if pump needs to be deactivated due to watchdog timeout

robot_control/src/utils/config.py

@@ -120,6 +120,18 @@ class ConfigParser:
             'exposure': vision_config.get('exposure', 0.1),
             'gain': vision_config.get('gain', 1.0)
         }
+    
+    def get_vacuum_config(self) -> dict:
+        """Get vacuum system configuration"""
+        vacuum_config = self.config.get('vacuum', {})
+        return {
+            '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_logging_config(self) -> dict:
         """Get logging configuration"""