feat: add movement playground for testing robot movement sequences; enhance GRBLHandler error handling

.vscode/launch.json

@@ -24,6 +24,13 @@
             "request": "launch",
             "module": "playgrounds.i2c_playground",
             "console": "integratedTerminal"
+        },
+        {
+            "name": "Movement_Playground",
+            "type": "debugpy",
+            "request": "launch",
+            "module": "playgrounds.movement_playground",
+            "console": "integratedTerminal"
         }
     ]
 }

playgrounds/movement_playground.py

@@ -0,0 +1,67 @@
+import asyncio
+from dataclasses import dataclass
+from robot_control.src.robot.movement import RobotMovement
+from robot_control.src.api.grbl_handler import GRBLHandler
+from robot_control.src.utils.config import RobotConfig, GRBLConfig, MovementConfig, GPIOConfig, MQTTConfig
+
+
+# grbl_config = GRBLConfig(port="debug", baudrate=115200)
+grbl_config = GRBLConfig(port="/dev/ttyACM1", baudrate=115200)
+test_config = MovementConfig(
+        speed=1000,
+        safe_height=10,
+        work_height=0
+    )
+
+grbl_handler = GRBLHandler(
+    grbl_config.port,
+    grbl_config.baudrate
+)
+movement = RobotMovement(test_config, grbl_handler)
+
+y_offset = 130
+
+positions = [
+    (453, 1028 - y_offset, 0),
+    (453, 1028 - y_offset, 107),
+    (453, 1028 - y_offset, 0),
+    (170, 760 - y_offset, 0),
+    (170, 760 - y_offset, 40),
+    (170, 760 - y_offset, 0),
+    (453, 600 - y_offset, 0), # neutral pos
+]
+
+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()
+
+async def perform_movement_sequence():
+    for pos in positions:
+        print(f"Moving to position {pos}...")
+        await movement.move_to_position(*pos)
+
+async def test_movement():
+    await grbl_handler.connect()
+    await movement.perform_homing()
+    
+    print("Press Enter to start movement sequence (or Ctrl+C to exit)...")
+    while True:
+        await wait_for_enter()
+        print("Starting movement sequence...")
+        await perform_movement_sequence()
+        print("\nPress Enter to repeat sequence (or Ctrl+C to exit)...")
+
+if __name__ == "__main__":
+    try:
+        asyncio.run(test_movement())
+    except KeyboardInterrupt:
+        print("\nExiting...")

robot_control/src/api/grbl_handler.py

@@ -80,14 +80,19 @@ class GRBLHandler:
         if self.debug:
             return None
             
-        if self.writer:
-            self.writer.write(b"?\n")
-            await self.writer.drain()
-            # '?' command returns status report and 'ok'
-            response = await self._process_response()
-            response = response + await self._process_response()
+        if not self.writer:
+            return None
+        
+        self.writer.write(b"?\n")
+        await self.writer.drain()
+        # '?' command returns status report and 'ok'
+        try:
+            response = str(await self._process_response())
+            response = response + str(await self._process_response())
             return response
-        return None
+        except Exception as e:
+            logger.error(f"Failed to get status: {str(e)}")
+            return None
 
     async def wait_until_idle(self, timeout_s, position: list[float] = None):
         """Wait until GRBL reports idle status

robot_control/src/robot/controller.py

@@ -35,7 +35,8 @@ class RobotController:
             self.config.grbl.port, 
             self.config.grbl.baudrate
         )
-        self.movement = RobotMovement(self.config, self.grbl_handler)
+        self.movement = RobotMovement(self.config.movement, self.grbl_handler)
+        self.movement.perform_homing()
 
         self.gpio = gpio_handler
         self.valve_pin = self.config.gpio.valve_pin

robot_control/src/robot/movement.py

@@ -4,7 +4,7 @@ import asyncio
 import pigpio
 from robot_control.src.api.grbl_handler import GRBLHandler
 import logging
-from robot_control.src.utils.config import RobotConfig
+from robot_control.src.utils.config import MovementConfig
 
 
 logger = logging.getLogger(__name__)
@@ -12,17 +12,16 @@ logger = logging.getLogger(__name__)
 class RobotMovement:
     """Class for controlling robot movements and end effector operations."""
 
-    def __init__(self, config: RobotConfig, grbl: GRBLHandler):
+    def __init__(self, config: MovementConfig, grbl: GRBLHandler):
         """Initialize the robot movement controller.
 
         Args:
             grbl (GRBLHandler): GRBL communication handler
-            config (RobotConfig): Configuration instance
-        """
-        self.config = config        
+            config (MovementConfig): Configuration instance
+        """    
 
         # Setting configs
-        self.speed: float = self.config.movement.speed
+        self.speed: float = config.speed
 
         # Basic variables for robot movement
         self.home_position: tuple[float, float, float] = (0.0, 0.0, 0.0)
@@ -118,7 +117,7 @@ class RobotMovement:
         self.is_moving = True
         try:
             logger.info("Homing machine...")
-            await self.grbl.send_and_wait_gcode(['$H'], position=self.current_position)
+            await self.grbl.send_gcode(['$H'])
             self.is_moving = False
             return True
         except Exception as e:

tests/test_robot.py

@@ -10,7 +10,7 @@ def robot_movement():
     config = ConfigParser().config
     grbl = config.grbl
     grbl_handler = GRBLHandler(grbl.port, grbl.baudrate)
-    return RobotMovement(config, grbl_handler)
+    return RobotMovement(config.movement, grbl_handler)
 
 @pytest.fixture
 def robot_controller():