add GRBL handler and integrate with movement; update config and requirements

robot-control/config/config.yaml

@@ -38,12 +38,16 @@ dropoff_grades:
     capacity_threshold: 0
 
 mqtt:
-  broker: localhost
+  broker: localhost # or debug
   port: 1883
   username: robot
   password: robot
   keepalive: 60
 
+grbl:
+  port: /dev/ttyUSB0 # or debug
+  baudrate: 115200
+
 system_settings:
   speed: 100.0
   acceleration: 500.0

robot-control/requirements.txt

@@ -8,4 +8,5 @@ PyYAML==6.0.1
 pytest==7.4.2
 pytest-asyncio==0.21.1
 pytest-cov==6.0.0
-httpx==0.25.0
+httpx==0.25.0
+paho-mqtt<2

robot-control/src/api/grbl_handler.py

@@ -0,0 +1,92 @@
+import asyncio
+import logging
+import os
+import socket
+from typing import List
+
+MOVE_RATE = 50
+APPROACHE_RATE = 10 
+CELL_CIRCLE_RADIUS = 0.35
+
+logger = logging.getLogger(__name__)
+
+class GRBLHandler:
+    def __init__(self, host: str, port: int):
+        self.host = host
+        self.port = port
+        self.debug = port == "debug"
+        
+        self.controller_active = asyncio.Event()
+        
+        self.reader = None
+        self.writer = None
+        self.position_callbacks = []
+
+    async def connect(self):
+        """Connect to GRBL controller"""
+        if self.debug:
+            logger.debug("GRBLHandler is not connected in debug mode")
+            return
+
+        logger.info("Connecting to GRBL...")
+        try:
+            self.reader, self.writer = await asyncio.open_connection(
+                self.host, self.port
+            )
+            await self._process_response()
+            logger.info("GRBL connected.")
+            await self.send_gcode(['$21=1', '$22=1', '$H', 'G90', 'G21', '$21=0'])
+        except Exception as e:
+            logger.error(f"Failed to connect to GRBL: {e}")
+            raise
+
+    async def send_gcode(self, commands: List[str]):
+        """Send GCODE commands"""
+        if self.debug:
+            logger.debug(f"G-Code commands [{*commands,}] not sent in debug mode")
+            return
+
+        self.controller_active.set()
+        try:
+            for cmd in commands:
+                self.writer.write(f"{cmd}\n".encode())
+                await self.writer.drain()
+                await self._process_response()
+        finally:
+            self.controller_active.clear()
+
+    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:
+                    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)
+                except Exception as e:
+                    logger.error(f"Failed to parse position: {e}")
+            
+            return decoded
+
+    def register_position_callback(self, callback):
+        """Register callback for position updates
+        
+        Args:
+            callback: Function taking tuple with X,Y,Z coordinates
+        """
+        self.position_callbacks.append(callback)
+
+    async def close(self):
+        """Close connection"""
+        if self.writer:
+            self.writer.close()
+            await self.writer.wait_closed()

robot-control/src/robot/config.py

@@ -96,3 +96,11 @@ class ConfigParser:
             'password': mqtt_config.get('password', None),
             'keepalive': mqtt_config.get('keepalive', 60)
         }
+    
+    def get_grbl_config(self) -> dict:
+        """Get GRBL handler configuration"""
+        grbl_config = self.config.get('grbl', {})
+        return {
+            'port': grbl_config.get('port', None),
+            'baudrate': grbl_config.get('baudrate', 115200),
+        }

robot-control/src/robot/controller.py

@@ -4,7 +4,8 @@ from typing import List, Tuple
 from .config import ConfigParser, SlotConfig
 from .movement import RobotMovement
 import logging
-from api.routes import MQTTHandler, Device
+from api.mqtt_handler import MQTTHandler, Device
+from api.grbl_handler import GRBLHandler
 
 logger = logging.getLogger(__name__)
 
@@ -36,6 +37,10 @@ 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.movement.set_speed(self.system_settings.speed)
         
@@ -46,7 +51,7 @@ class RobotController:
         self.gripper_occupied = False
         
         # Initialize MQTT handler
-        mqtt_config = self.config.get_mqtt_config()  # Add this to config parser
+        mqtt_config = self.config.get_mqtt_config()
         self.mqtt_handler = MQTTHandler(
             broker=mqtt_config.get('broker'),
             port=mqtt_config.get('port'),

robot-control/src/robot/movement.py

@@ -1,28 +1,36 @@
 from typing import Tuple, Optional
 import time
-
-# TODO[SG]: Implement actual robot movement commands. This is just a placeholder.
+import asyncio
+from ..api.grbl_handler import GRBLHandler
 
 class RobotMovement:
-    def __init__(self):
-        self.current_position: Tuple[float, float, float] = (0.0, 0.0, 0.0)  # x, y, z
-        self.speed: float = 100.0  # Default speed in mm/s
+    def __init__(self, grbl: GRBLHandler):
+        self.grbl = grbl
+        self.current_position: Tuple[float, float, float] = (0.0, 0.0, 0.0)
+        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.gripper_state: bool = False  # False = open, True = closed
+        
+        # Register for position updates
+        self.grbl.register_position_callback(self._position_callback)
 
-    def set_speed(self, speed: float) -> None:
-        """Set the movement speed in mm/s."""
-        if speed <= 0:
+    def _position_callback(self, pos: dict):
+        self.current_position = (pos['X'], pos['Y'], pos['Z'])
+        
+    def set_speed(self, speed_mms: float) -> None:
+        if speed_mms <= 0:
             raise ValueError("Speed must be positive")
-        self.speed = speed
+        self.speed = speed_mms
+        # Set GRBL feed rate
+        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:
-        """Move to absolute position. Returns True if movement successful."""
         try:
             self.is_moving = True
-            # Here you would implement actual robot movement commands
-            self.current_position = (x, y, z)
+            # Send G-code move command
+            await self.grbl.send_gcode([
+                f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{self.speed * 60}'
+            ])
             self.is_moving = False
             return True
         except Exception as e:
@@ -30,46 +38,33 @@ class RobotMovement:
             raise RuntimeError(f"Movement failed: {str(e)}")
 
     async def move_relative(self, dx: float, dy: float, dz: float) -> bool:
-        """Move relative to current position."""
-        new_x = self.current_position[0] + dx
-        new_y = self.current_position[1] + dy
-        new_z = self.current_position[2] + dz
-        return await self.move_to_position(new_x, new_y, new_z)
+        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:
-        """Move to home position."""
-        return await self.move_to_position(*self.home_position)
+        self.is_moving = True
+        try:
+            await self.grbl.send_gcode(['$H'])  # GRBL home command
+            self.is_moving = False
+            return True
+        except Exception as e:
+            self.is_moving = False
+            raise RuntimeError(f"Homing failed: {str(e)}")
 
     def get_position(self) -> Tuple[float, float, float]:
-        """Get current position."""
         return self.current_position
 
     async def stop(self) -> None:
         """Emergency stop."""
         self.is_moving = False
-        # Implement emergency stop command here
+        await self.grbl.send_gcode(['!', 'M5'])  # Feed hold + stop endeffector
 
-    def wait_until_idle(self, timeout: Optional[float] = None) -> bool:
-        """Wait until movement is complete or timeout (in seconds) is reached."""
+    async def wait_until_idle(self, timeout: Optional[float] = None) -> bool:
         start_time = time.time()
         while self.is_moving:
             if timeout and (time.time() - start_time) > timeout:
                 return False
-            time.sleep(0.1)
-        return True
-
-    async def activate_gripper(self) -> bool:
-        """Close gripper to grip cell. Returns True if successful."""
-        try:
-            self.gripper_state = True
-            return True
-        except Exception as e:
-            raise RuntimeError(f"Failed to close gripper: {str(e)}")
-
-    async def deactivate_gripper(self) -> bool:
-        """Open gripper to release cell. Returns True if successful."""
-        try:
-            self.gripper_state = False
-            return True
-        except Exception as e:
-            raise RuntimeError(f"Failed to open gripper: {str(e)}")
+            await asyncio.sleep(0.1)  # Non-blocking wait
+        return True

robot-control/tests/test_api.py

@@ -1,6 +1,6 @@
 import pytest
 from fastapi.testclient import TestClient
-from api.routes import MQTTHandler, Device, MeasurementResult, mqtt
+from api.mqtt_handler import MQTTHandler, Device, MeasurementResult, mqtt
 from unittest.mock import Mock, patch
 
 @pytest.fixture
@@ -16,6 +16,12 @@ def mqtt_handler(mock_mqtt_client):
     yield handler
     handler.cleanup()
 
+# @pytest.fixture
+# def real_mqtt_handler():
+#     handler = MQTTHandler(username="robot", password="robot")
+#     yield handler
+#     handler.cleanup()
+
 class TestMQTTHandler:
     def test_device_registration(self, mqtt_handler:MQTTHandler):
         device = Device("test_device", 4)