add endeffector; refactor logging setup and configuration;

.gitignore

@@ -3,3 +3,6 @@
 .pyc
 robot-control/src/robot_control.egg-info/
 __pycache__
+.coverage
+.python-version
+logs/robot.log

.vscode/launch.json

@@ -0,0 +1,22 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Python Debugger: Current File",
+            "type": "debugpy",
+            "request": "launch",
+            "program": "${file}",
+            "console": "integratedTerminal"
+        },
+        {
+            "name": "Python Debugger: Main",
+            "type": "debugpy",
+            "request": "launch",
+            "program": "${workspaceFolder}/robot-control/src/main.py",
+            "console": "integratedTerminal"
+        }
+    ]
+}

robot-control/config/config.yaml

@@ -45,10 +45,17 @@ mqtt:
   keepalive: 60
 
 grbl:
-  port: /dev/ttyUSB0 # or debug
+  port: /dev/ttyACM0 # or debug
   baudrate: 115200
 
 system_settings:
   speed: 100.0
   acceleration: 500.0
   safe_height: 20.0
+
+logging:
+  level: DEBUG
+  file_path: logs/robot.log
+  max_file_size_mb: 10
+  backup_count: 5
+  console_output: true

robot-control/src/api/grbl_handler.py

@@ -1,20 +1,19 @@
 import asyncio
-import logging
-import os
-import socket
+import serial_asyncio
 from typing import List
+from utils.logging import LoggerSingleton
 
 MOVE_RATE = 50
 APPROACHE_RATE = 10 
 CELL_CIRCLE_RADIUS = 0.35
 
-logger = logging.getLogger(__name__)
+logger = LoggerSingleton.get_logger()
 
 class GRBLHandler:
-    def __init__(self, host: str, port: int):
-        self.host = host
+    def __init__(self, port: str, baudrate: int):
         self.port = port
         self.debug = port == "debug"
+        self.baudrate = baudrate
         
         self.controller_active = asyncio.Event()
         
@@ -30,8 +29,9 @@ class GRBLHandler:
 
         logger.info("Connecting to GRBL...")
         try:
-            self.reader, self.writer = await asyncio.open_connection(
-                self.host, self.port
+            self.reader, self.writer = await serial_asyncio.open_serial_connection(
+                url = self.port,
+                baudrate = self.baudrate
             )
             await self._process_response()
             logger.info("GRBL connected.")
@@ -49,6 +49,7 @@ class GRBLHandler:
         self.controller_active.set()
         try:
             for cmd in commands:
+                logger.debug(f"Sending G-Code command: {cmd}")
                 self.writer.write(f"{cmd}\n".encode())
                 await self.writer.drain()
                 await self._process_response()
@@ -58,13 +59,13 @@ class GRBLHandler:
     async def _process_response(self):
         """Process GRBL responses"""
         if self.reader:
-            response = await self.reader.readline()
-            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:
-                try:
+            try:
+                response = await asyncio.wait_for(self.reader.readline(), timeout=2.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)
@@ -72,10 +73,14 @@ class GRBLHandler:
                     # Notify all registered callbacks
                     for callback in self.position_callbacks:
                         callback(self.current_position)
-                except Exception as e:
-                    logger.error(f"Failed to parse position: {e}")
-            
-            return decoded
+                
+                return decoded
+            except asyncio.TimeoutError:
+                logger.warning("Timeout waiting for GRBL response")
+                return None
+            except Exception as e:
+                logger.error(f"Failed to parse position: {e}")
+                return None
 
     def register_position_callback(self, callback):
         """Register callback for position updates

robot-control/src/api/mqtt_handler.py

@@ -2,9 +2,9 @@ import paho.mqtt.client as mqtt
 from typing import Dict, List, Callable
 from pydantic import BaseModel
 import json
-import logging
+from utils.logging import LoggerSingleton
 
-logger = logging.getLogger(__name__)
+logger = LoggerSingleton.get_logger()
 
 class Device:
     def __init__(self, device_id: str, num_slots: int):
@@ -76,7 +76,7 @@ class MQTTHandler:
                     
             elif topic.startswith("soa/"):
                 logger.info(f"SOA update for device {device_id}: {payload}")
-                # [SG]TODO: Handle SOA update here
+                # TODO[SG]: Handle SOA update here
                 
         except Exception as e:
             logger.error(f"Error processing message: {e}")

robot-control/src/main.py

@@ -1,26 +1,36 @@
 import asyncio
+from utils.logging import LoggerSingleton
 from fastapi import FastAPI
 from robot.controller import RobotController, Cell
-from robot.config import ConfigParser
+from utils.config import ConfigParser
 from vision.datamatrix import DataMatrixReader
 
 class LoaderSystem:
     def __init__(self):
         self.config = ConfigParser()
+        self.logger = LoggerSingleton.get_logger(self.config)
         self.controller = RobotController(self.config)
         self.vision = DataMatrixReader()
+        self.logger.info("Initializing LoaderSystem")
+        self.vision.initialize()
 
     async def run(self):
+        self.logger.info("Connecting to robot controller")
+        await self.controller.connect()
+
         while True:
             # Main robot control loop
             while True:
                 slot = self.controller.get_next_free_slot()
                 if not slot:
+                    self.logger.debug("No free slots available")
                     break
                 # Pick and place new cell
-                cell_id = await self.vision.read_datamatrix()
+                cell_id = self.vision.read_datamatrix()
                 if not cell_id:
+                    self.logger.debug("Failed to read datamatrix")
                     break
+                self.logger.info(f"Processing cell {cell_id}")
                 cell = Cell(cell_id)
                 await self.controller.pick_cell_from_feeder()
                 await self.controller.insert_cell_to_slot(cell, slot)

robot-control/src/robot/controller.py

@@ -1,13 +1,13 @@
 from dataclasses import dataclass
 from enum import Enum
 from typing import List, Tuple
-from .config import ConfigParser, SlotConfig
-from .movement import RobotMovement
-import logging
+from utils.config import ConfigParser, SlotConfig
+from robot.movement import RobotMovement
+from utils.logging import LoggerSingleton
 from api.mqtt_handler import MQTTHandler, Device
 from api.grbl_handler import GRBLHandler
 
-logger = logging.getLogger(__name__)
+logger = LoggerSingleton.get_logger()
 
 class CellStatus(Enum):
     WAITING = "waiting"
@@ -18,7 +18,7 @@ class CellStatus(Enum):
 @dataclass
 class Cell:
     id: str
-    status: CellStatus
+    status: CellStatus = CellStatus.WAITING
     measurement_slot: int = None
     capacity: float = None
 
@@ -41,8 +41,7 @@ class RobotController:
         grbl_config = self.config.get_grbl_config()
 
         self.grbl_handler = GRBLHandler(grbl_config.get('port'), grbl_config.get('baudrate'))
-        self.movement = RobotMovement()
-        self.movement.set_speed(self.system_settings.speed)
+        self.movement = RobotMovement(self.grbl_handler)
         
         # Initialize with configured values
         self.total_slots = sum(len(device.slots) for device in self.devices)
@@ -65,6 +64,10 @@ class RobotController:
                 Device(device_id=device.id, num_slots=len(device.slots))
             )
 
+    async def connect(self):
+        await self.grbl_handler.connect()
+        await self.movement.set_speed(self.system_settings.speed)
+
     async def cleanup(self):
         """Cleanup resources on shutdown"""
         # await self.movement.cleanup() TODO[SG]: Implement cleanup method in movement.py
@@ -76,14 +79,14 @@ class RobotController:
             return False
             
         try:
-            feeder_pos = self.feeder.pickup_position
+            feeder_pos = self.feeder.approach_position
             x, y, z = feeder_pos
             # Move to safe height first
             await self.movement.move_to_position(x, y, self.system_settings.safe_height)
             # Move to pickup position
             await self.movement.move_to_position(x, y, z)
             # Grip cell
-            if await self.movement.activate_gripper():
+            if await self.movement.activate_endeffector():
                 raise RuntimeError("Failed to grip cell")
             self.gripper_occupied = True
 
@@ -93,7 +96,7 @@ class RobotController:
             return True
         except Exception as e:
             logger.error(f"Failed to pick cell from feeder: {str(e)}")
-            await self.movement.deactivate_gripper() # Try to deactivate to avoid stuck gripper
+            await self.movement.deactivate_endeffector() # TODO [SG]: Should deactivate here?
             self.gripper_occupied = False
             return False
 

robot-control/src/robot/movement.py

@@ -1,7 +1,11 @@
 from typing import Tuple, Optional
 import time
 import asyncio
-from ..api.grbl_handler import GRBLHandler
+from api.grbl_handler import GRBLHandler
+from utils.logging import LoggerSingleton
+
+
+logger = LoggerSingleton.get_logger()
 
 class RobotMovement:
     def __init__(self, grbl: GRBLHandler):
@@ -10,6 +14,8 @@ class RobotMovement:
         self.speed: float = 100.0 # TODO [SG]: Set at beginning?
         self.is_moving: bool = False
         self.home_position: Tuple[float, float, float] = (0.0, 0.0, 0.0)
+        self.endeffector_active = False
+        self.pump_speed = 1000  # Default pump speed/power
         
         # Register for position updates
         self.grbl.register_position_callback(self._position_callback)
@@ -17,12 +23,12 @@ class RobotMovement:
     def _position_callback(self, pos: dict):
         self.current_position = (pos['X'], pos['Y'], pos['Z'])
         
-    def set_speed(self, speed_mms: float) -> None:
+    async def set_speed(self, speed_mms: float) -> None:
         if speed_mms <= 0:
             raise ValueError("Speed must be positive")
         self.speed = speed_mms
         # Set GRBL feed rate
-        self.grbl.send_gcode([f'F{speed_mms * 60}'])  # Convert mm/s to mm/min
+        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) -> bool:
         try:
@@ -55,11 +61,40 @@ class RobotMovement:
 
     def get_position(self) -> Tuple[float, float, float]:
         return self.current_position
+    
+    async def activate_endeffector(self, speed: Optional[int] = None) -> bool:
+        """Activate the pump end effector.
+        
+        Args:
+            speed: Optional pump speed (0-1000)
+        """
+        try:
+            if speed is not None:
+                self.pump_speed = max(0, min(1000, speed))
+            await self.grbl.send_gcode([f'M3 S{self.pump_speed}'])
+            self.endeffector_active = True
+            return True
+        except Exception as e:
+            raise RuntimeError(f"Failed to activate end effector: {str(e)}")
+
+    async def deactivate_endeffector(self) -> bool:
+        """Deactivate the pump end effector."""
+        try:
+            await self.grbl.send_gcode(['M5'])
+            self.endeffector_active = False
+            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
 
     async def stop(self) -> None:
         """Emergency stop."""
         self.is_moving = False
-        await self.grbl.send_gcode(['!', 'M5'])  # Feed hold + stop endeffector
+        self.endeffector_active = False
+        await self.grbl.send_gcode(['!', 'M5'])  # Feed hold + stop pump
 
     async def wait_until_idle(self, timeout: Optional[float] = None) -> bool:
         start_time = time.time()

robot-control/src/robot/config.py → robot-control/src/utils/config.py

@@ -104,3 +104,14 @@ class ConfigParser:
             'port': grbl_config.get('port', None),
             'baudrate': grbl_config.get('baudrate', 115200),
         }
+
+    def get_logging_config(self) -> dict:
+        """Get logging configuration"""
+        log_config = self.config.get('logging', {})
+        return {
+            'level': log_config.get('level', 'INFO'),
+            'file_path': log_config.get('file_path', 'logs/robot.log'),
+            'max_file_size_mb': log_config.get('max_file_size_mb', 10),
+            'backup_count': log_config.get('backup_count', 5),
+            'console_output': log_config.get('console_output', True)
+        }

robot-control/src/utils/logging.py

@@ -0,0 +1,41 @@
+import logging
+from logging.handlers import RotatingFileHandler
+import os
+from typing import Optional
+from utils.config import ConfigParser
+
+class LoggerSingleton:
+    _instance: Optional[logging.Logger] = None
+
+    @classmethod
+    def get_logger(cls, config: Optional[ConfigParser] = None) -> logging.Logger:
+        if config is None:
+            config = ConfigParser()
+        if cls._instance is None and config is not None:
+            cls._instance = cls._setup_logger(config)
+        return cls._instance
+
+    @staticmethod
+    def _setup_logger(config: ConfigParser) -> logging.Logger:
+        log_config = config.get_logging_config()
+        logger = logging.getLogger('robot_control')
+        logger.setLevel(getattr(logging, log_config['level']))
+
+        os.makedirs(os.path.dirname(log_config['file_path']), exist_ok=True)
+
+        file_handler = RotatingFileHandler(
+            log_config['file_path'],
+            maxBytes=log_config['max_file_size_mb'] * 1024 * 1024,
+            backupCount=log_config['backup_count']
+        )
+        file_formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
+        file_handler.setFormatter(file_formatter)
+        logger.addHandler(file_handler)
+
+        if log_config['console_output']:
+            console_handler = logging.StreamHandler()
+            console_formatter = logging.Formatter('%(levelname)s: %(message)s')
+            console_handler.setFormatter(console_formatter)
+            logger.addHandler(console_handler)
+
+        return logger

robot-control/src/vision/datamatrix.py

@@ -1,10 +1,9 @@
 import cv2
 from pylibdmtx.pylibdmtx import decode
-import logging
-import os
+from utils.logging import LoggerSingleton
 from pathlib import Path
 
-logger = logging.getLogger(__name__)
+logger = LoggerSingleton.get_logger()
 
 class TestCamera:
     def __init__(self, test_files_path: str):

robot-control/tests/test_robot.py

@@ -1,7 +1,7 @@
 import pytest
 from robot.movement import RobotMovement
 from robot.controller import RobotController, Cell, CellStatus
-from robot.config import ConfigParser
+from utils.config import ConfigParser
 
 @pytest.fixture
 def robot_movement():