ref/fix: improve GPIO handling and cleanup methods; fix GRBL connection error/response handling; update config with positoins and vaccum values

enter in integration test for loop

main.py

@@ -20,7 +20,7 @@ class LoaderSystem:
         if gpio_config.debug:
             self.gpio = MockGPIO()
         else:
-            self.gpio = PiGPIO([gpio_config.pump_pin, gpio_config.valve_pin], [])
+            self.gpio = PiGPIO(out_pins=[gpio_config.pump_pin, gpio_config.valve_pin])
 
         self.logger = logging.getLogger(__name__)
         self.controller = RobotController(self.config, self.gpio)
@@ -38,11 +38,14 @@ class LoaderSystem:
 
     async def run(self):
         await self.controller.connect()
-
-        await asyncio.gather(
-            self._loader_loop(),
-            self._poll_i2c_channels()
-        )
+        try:
+            await asyncio.gather(
+                self._loader_loop(),
+                self._poll_i2c_channels()
+            )
+        finally:
+            self.cleanup()
+            logger.info("Cleaning up resources...")
 
     async def _poll_i2c_channels(self):
         while True:
@@ -86,8 +89,8 @@ class LoaderSystem:
             # 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
+    def cleanup(self):
+        self.gpio.cleanup()  # Ensure PumpController cleans up gpio
 
 if __name__ == "__main__":
     loader_system = LoaderSystem()

playgrounds/integration_test.py

@@ -1,3 +1,5 @@
+import signal
+import sys
 import asyncio
 import logging
 from robot_control.src.robot.controller import RobotController, Cell, CellStatus
@@ -14,12 +16,26 @@ logging.basicConfig(
 )
 
 logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
 
 """
 This is a test script for the loader system.
 It initializes the robot controller, vision system, and I2C handler,
 """
 
+async def wait_for_enter():
+    # Use asyncio.Event to coordinate between input and async code
+    event = asyncio.Event()
+    
+    def input_callback():
+        input()  # Wait for Enter
+        event.set()
+    
+    # Run input in a separate thread to not block the event loop
+    loop = asyncio.get_running_loop()
+    await loop.run_in_executor(None, input_callback)
+    await event.wait()
+
 class LoaderSystem:
     def __init__(self):
         self.config = ConfigParser().config
@@ -27,7 +43,7 @@ class LoaderSystem:
         if gpio_config.debug:
             self.gpio = MockGPIO()
         else:
-            self.gpio = PiGPIO([gpio_config.pump_pin, gpio_config.valve_pin], [])
+            self.gpio = PiGPIO(out_pins=[gpio_config.pump_pin, gpio_config.valve_pin])
 
         self.controller = RobotController(self.config, self.gpio)
         # self.vision = DataMatrixReader(self.config)
@@ -49,7 +65,6 @@ class LoaderSystem:
 
     async def run(self):
         await self.controller.connect()
-
         await asyncio.gather(
             self._loader_loop(),
             self._poll_i2c_channels()
@@ -61,8 +76,8 @@ class LoaderSystem:
                 readings = await self.i2c.read_channels([1, 3, 4])
                 for channel, value in readings.items():
                     logger.debug(f"Channel {channel} reading: {value}")
-                    # if channel == 3:  # Pressure reading
-                    #     self.pump_controller.handle_tank_reading(value)
+                    if channel == 3:  # Pressure reading
+                        self.pump_controller.handle_tank_reading(value)
                     if channel == 4:
                         state = self.pump_controller.check_endeffector_state(value)
                         self.controller.set_suction_state(state)
@@ -71,16 +86,34 @@ class LoaderSystem:
             await asyncio.sleep(1)  # Poll every second
 
     async def _loader_loop(self):
-        await self.controller.pick_cell_from_slot(self.test_slot)
-        await self.controller.dropoff_cell(self.test_dropoff)
+        while True:
+            await wait_for_enter()
+            logger.info("Starting movement sequence...")
+            await self.controller.pick_cell_from_slot(self.test_slot)
+            await self.controller.dropoff_cell(self.test_dropoff)
 
-        await self.controller.pick_cell_from_feeder()
-        cell = Cell(id="test_cell", status=CellStatus.WAITING)
-        await self.controller.insert_cell_to_slot(cell, self.test_slot)
+            await self.controller.pick_cell_from_feeder()
+            cell = Cell(id="test_cell", status=CellStatus.WAITING)
+            await self.controller.insert_cell_to_slot(cell, self.test_slot)
+            logger.info("\nPress Enter to repeat sequence (or Ctrl+C to exit)...")
 
-    def __del__(self):
-        self.pump_controller.cleanup()  # Ensure PumpController cleans up gpio
+    async def cleanup(self):
+        logger.info("Cleaning up resources...")
+        await self.controller.cleanup()
+        self.gpio.cleanup()
 
 if __name__ == "__main__":
     loader_system = LoaderSystem()
+
+    async def shutdown():
+        logger.info("Shutting down...")
+        await loader_system.cleanup()
+        sys.exit(0)
+
+    def handle_signal(sig, frame):
+        asyncio.get_event_loop().create_task(shutdown())
+
+    signal.signal(signal.SIGINT, handle_signal)
+    signal.signal(signal.SIGTERM, handle_signal)
+
     asyncio.run(loader_system.run())

robot_control/config/config.yaml

@@ -1,6 +1,6 @@
 measurement_devices:
   - id: device_1
-    position: [378, 898, 107]
+    position: [375.5, 898, 107]
     slots:
       - position: [10, 0, 0]
         occupied: False
@@ -28,14 +28,14 @@ measurement_devices:
         slot_id: 7
 
 feeder:
-  position: [30, 621.5, 47.5]
+  position: [27.5, 621.5, 47.5]
 
 dropoff_grades:
   - id: accepted
-    position: [170, 630, 40]
+    position: [167.5, 630, 40]
     capacity_threshold: 0.8
   - id: rejected
-    position: [170, 630, 40]
+    position: [167.5, 630, 40]
     capacity_threshold: 0
 
 mqtt:
@@ -46,7 +46,7 @@ mqtt:
   keepalive: 60
 
 grbl:
-  port: /dev/ttyACM0 # or debug
+  port: /dev/ttyUSB0 # or debug
   # port: debug
   baudrate: 115200
 
@@ -59,10 +59,10 @@ vision:
   bbox: [100, 100,  200, 200]  # (x, y, width, height)
 
 vacuum:
-  min_pressure_bar: 0.14
-  max_pressure_bar: 0.7
+  min_pressure_bar: 0.13
+  max_pressure_bar: 0.6
   max_pump_time_s: 20
-  gripping_threshold_bar: 0.7
+  gripping_threshold_bar: 0.6
   pump_watchdog_timeout_s: 30
 
 gpio:

robot_control/src/api/gpio.py

@@ -32,7 +32,10 @@ class PiGPIO(GPIOInterface):
     def get_pin(self, pin: int) -> int:
         return self.pi.read(pin)
     
-    def __del__(self):
+    def cleanup(self):
+        if self.pi.connected:
+            for pin in self.out_pins:
+                self.set_pin(pin, 0)
         self.pi.stop()
 
 class MockGPIO(GPIOInterface):

robot_control/src/api/grbl_handler.py

@@ -9,6 +9,8 @@ MOVE_RATE = 50
 APPROACHE_RATE = 10 
 CELL_CIRCLE_RADIUS = 0.35
 
+HOMING_TIMEOUT_S = 80
+
 logger = logging.getLogger(__name__)
 
 class GRBLHandler:
@@ -38,21 +40,49 @@ class GRBLHandler:
         except serial_asyncio.serial.SerialException as e:
             raise serial_asyncio.serial.SerialException(f"Failed to connect to robot: {str(e)}")
 
-        init_response = []
-        for _ in range(3): # Flush initial responses
-            init_response.append(await self._process_response())
-        if not any(isinstance(response, collections.Iterable) and "Grbl" in response for response in init_response):
+        init_response = await self._process_response()
+        if not any(isinstance(response, collections.abc.Iterable) and "Grbl" in response for response in init_response):
             raise RuntimeError("Failed to connect to GRBL")
         
         logger.info("GRBL connected.")
+
+        # Set GRBL settings:
         # Enable homing ($22=1)
         # Run homing cycle ($H)
         # Set absolute positioning mode (G90)
         # Set units to millimeters (G21) 
-        # await self.send_gcode(['$22=1', '$H', 'G90', 'G21'])
-        await self.send_gcode(['$22=1', '$X', 'G90', 'G21']) # This skips homing
+        await self.send_gcode(['$22=1']) 
+        await self.send_gcode(['$H'], HOMING_TIMEOUT_S) 
+        await self.send_gcode(['G90', 'G21']) 
+
+        # if current pos not 0 set working pos to current pos
+        # (should not be done every time to save eeprom writes)
+        status_response = await self.get_status()
+        current_pos = (0.0, 0.0, 0.0)
+        if status_response and 'WPos:' in status_response:
+            pos_str = status_response.split('WPos:')[1].split('|')[0]
+            x, y, z = map(float, pos_str.split(','))
+            current_pos = (x, y, z)
+        # If not at (0,0,0), set working position to current position
+        if any(abs(coord) > 1e-6 for coord in current_pos):
+            await self.send_gcode([f'G10 L20 P1 X0 Y0 Z0'])
+            logger.info(f"Set working position to current position: {current_pos}")
+
+    async def reset(self):
+        """Reset GRBL controller"""
+        if self.debug:
+            logger.debug("GRBLHandler is not reset in debug mode")
+            return
 
-    async def send_gcode(self, commands: List[str]):
+        if self.writer:
+            self.writer.write(b"\x18")  # Soft-reset GRBL
+            await self.writer.drain()
+            await asyncio.sleep(0.5)  # Wait for GRBL to reset
+            await self.close()
+            
+        await self.connect()  # Reconnect after reset
+
+    async def send_gcode(self, commands: List[str], timeout_s = 6):
         """Send GCODE commands"""
         if self.debug:
             logger.debug(f"G-Code commands [{*commands,}] not sent in debug mode")
@@ -64,7 +94,7 @@ class GRBLHandler:
                 logger.debug(f"Sending G-Code command: {cmd}")
                 self.writer.write(f"{cmd}\n".encode())
                 await self.writer.drain()
-                if not (await self._process_response()):
+                if not (await self._process_response(timeout_s)):
                     raise RuntimeError("Did not receive response from GRBL")
         except RuntimeError as e:
             logger.error(f"Failed to send G-Code commands: {str(e)}")
@@ -83,16 +113,26 @@ class GRBLHandler:
         if not self.writer:
             return None
         
-        self.writer.write(b"?\n")
-        await self.writer.drain()
-        # '?' command returns status report and 'ok'
+        response_lines = None
         try:
-            response = str(await self._process_response())
-            response = response + str(await self._process_response())
-            return response
+            self.writer.write(b"?") # '?' command returns status report
+            await self.writer.drain()
+            response_lines = str(await self._process_response())
         except Exception as e:
             logger.error(f"Failed to get status: {str(e)}")
             return None
+        
+        if not response_lines:
+            return None
+        
+        for line in response_lines:
+            if not 'ALARM' in line:
+                continue
+            if ":1" in line:
+                raise RuntimeError("Hard Limit was triggered!")
+            raise RuntimeError(f"Grbl threw ALARM: {line}")
+        
+        return response_lines
 
     async def wait_until_idle(self, timeout_s, position: list[float] = None):
         """Wait until GRBL reports idle status
@@ -106,11 +146,12 @@ class GRBLHandler:
             return
             
         start = datetime.datetime.now()
+        # response = await self.get_status() # First response can still be idle
         while True:
             response = await self.get_status()
-            if response and 'MPos:' in response:
-                # Parse position from status reports (<Idle|MPos:0.000,0.000,0.000|...>)
-                pos_str = response.split('MPos:')[1].split('|')[0]
+            if response and 'WPos:' in response:
+                # Parse position from status reports (<Idle|WPos:0.000,0.000,0.000|...>)
+                pos_str = response.split('WPos:')[1].split('|')[0]
                 if position is not None:
                     x, y, z = map(float, pos_str.split(','))
                     position = (x,y,z)
@@ -118,19 +159,13 @@ class GRBLHandler:
             if response and "Idle" in response:
                 logger.debug("Movement complete.\nContinuing...")
                 break
-
-            if response and "ALARM" in response:
-                if ":1" in response:
-                    raise RuntimeError("Hard Limit was triggered!")
-                else:
-                    raise RuntimeError(f"Grbl threw ALARM: {response}")
                     
             now = datetime.datetime.now()
             if (now - start).total_seconds() > timeout_s:
                 logger.error("Waiting on idle took too long!")
                 raise TimeoutError("GRBL did not report idle status")
                 
-            await asyncio.sleep(0.2)  # Async delay to prevent flooding
+            await asyncio.sleep(0.5)  # Async delay to prevent flooding
 
     async def send_and_wait_gcode(self, commands: List[str], timeout_s=60, position: list[float] = None):
         """Send GCODE commands and wait until machine is idle"""
@@ -138,29 +173,41 @@ class GRBLHandler:
         await asyncio.sleep(0.2)  # Delay to allow GRBL to process commands
         await self.wait_until_idle(timeout_s, position)
 
-    async def _process_response(self):
+    def _check_pos_callbacks(self, line):
+        # Parse position from status reports (<Idle|WPos:0.000,0.000,0.000|...>)
+        if line.startswith('<') and 'WPos:' in line:
+            pos_str = line.split('WPos:')[1].split('|')[0]
+            x, y, z = map(float, pos_str.split(','))
+            self.current_position = (x,y,z)
+            
+            # Notify all registered callbacks
+            for callback in self.position_callbacks:
+                callback(self.current_position)
+
+    async def _process_response(self, timeout_s=6.0):
         """Process GRBL responses"""
         if self.reader:
-            try:
-                response = await asyncio.wait_for(self.reader.readuntil(), timeout=4.0)  # 2 second timeout
-                decoded = response.strip().decode("utf-8")
-                logger.debug(f"G-Code response: {decoded}")
-                
-                # Parse position from status reports (<Idle|MPos:0.000,0.000,0.000|...>)
-                if decoded.startswith('<') and 'MPos:' in decoded:
-                    pos_str = decoded.split('MPos:')[1].split('|')[0]
-                    x, y, z = map(float, pos_str.split(','))
-                    self.current_position = (x,y,z)
-                    
-                    # Notify all registered callbacks
-                    for callback in self.position_callbacks:
-                        callback(self.current_position)
-                
-                return decoded
-            except asyncio.TimeoutError as e:
-                logger.warning("Timeout waiting for GRBL response")
-            except Exception as e:
-                raise RuntimeError(f"Failed to process response: {e}")
+            response_lines = []
+            first = True
+            while True:
+                try:
+                    current_timeout = timeout_s if first else 0.1
+                    line = await asyncio.wait_for(self.reader.readuntil(), timeout=current_timeout)
+                    decoded = line.strip().decode("utf-8")
+                    self._check_pos_callbacks(decoded)
+                    logger.debug(f"G-Code response: {decoded}")
+                    response_lines.append(decoded)
+                    first = False
+                except asyncio.TimeoutError:
+                    # No more data available right now
+                    break
+                except Exception as e:
+                    raise RuntimeError(f"Failed to process response: {e}")
+            
+            if not response_lines:
+                logger.warning(f"No GRBL response received! ({timeout_s}s)")
+
+            return response_lines
 
     def register_position_callback(self, callback):
         """Register callback for position updates
@@ -174,4 +221,6 @@ class GRBLHandler:
         """Close connection"""
         if self.writer:
             self.writer.close()
-            await self.writer.wait_closed()
+            await self.writer.wait_closed()
+        self.reader = None
+        self.writer = None

robot_control/src/robot/controller.py

@@ -91,12 +91,17 @@ class RobotController:
         self.suction_state = state
 
     async def connect(self):
-        await self.grbl_handler.connect()
+        try:
+            await self.grbl_handler.connect()
+        except RuntimeError as e:
+            logger.info(f"Connection failed. Try to reset...")
+            await self.grbl_handler.reset()
+
         await self.movement.set_speed(self.config.movement.speed)
 
     async def cleanup(self):
         """Cleanup resources on shutdown"""
-        # await self.movement.cleanup() TODO[SG]: Implement cleanup method in movement.py
+        await self.grbl_handler.close()
         self.mqtt_handler.cleanup()
 
     def add_cell(self, cell_id: str):

robot_control/src/robot/pump_controller.py

@@ -66,6 +66,3 @@ class PumpController:
             self.watchdog_trigger_time = None
             self.last_activated_time = None
 
-    def cleanup(self):
-        self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
-        logger.info("Pigpio cleanup completed")