enhance GRBL handler with idle status check and command waiting; improve logging and error handling

robot-control/src/api/grbl_handler.py

@@ -2,6 +2,7 @@ import asyncio
 import serial_asyncio
 from typing import List
 from utils.logging import LoggerSingleton
+import datetime
 
 MOVE_RATE = 50
 APPROACHE_RATE = 10 
@@ -33,9 +34,15 @@ class GRBLHandler:
                 url = self.port,
                 baudrate = self.baudrate
             )
-            await self._process_response()
+            for _ in range(3):
+                await self._process_response()
             logger.info("GRBL connected.")
-            await self.send_gcode(['$21=1', '$22=1', '$H', 'G90', 'G21', '$21=0'])
+            # 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'])
         except Exception as e:
             logger.error(f"Failed to connect to GRBL: {e}")
             raise
@@ -56,6 +63,35 @@ class GRBLHandler:
         finally:
             self.controller_active.clear()
 
+    async def wait_until_idle(self, timeout_s=None):
+        """Wait until GRBL reports idle status"""
+        timeout_s = timeout_s or 50
+        if self.debug:
+            return
+            
+        start = datetime.datetime.now()
+        while True:
+            if self.writer:
+                self.writer.write(b"?\n")
+                await self.writer.drain()
+                response = await self._process_response()
+                
+                if response and "Idle" in response:
+                    logger.debug("Movement complete.\nContinuing...")
+                    break
+                    
+            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
+
+    async def send_and_wait_gcode(self, commands: List[str], timeout_s=None):
+        """Send GCODE commands and wait until machine is idle"""
+        await self.send_gcode(commands)
+        await self.wait_until_idle(timeout_s)
+
     async def _process_response(self):
         """Process GRBL responses"""
         if self.reader:

robot-control/src/main.py

@@ -18,17 +18,19 @@ class LoaderSystem:
         self.logger.info("Connecting to robot controller")
         await self.controller.connect()
 
+        # Main robot control loop
         while True:
-            # Main robot control loop
+            await asyncio.sleep(0.1) # avoid busy loop
+
+            #Check for free slots 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 = self.vision.read_datamatrix()
                 if not cell_id:
-                    self.logger.debug("Failed to read datamatrix")
+                    self.logger.debug("No cell detected")
                     break
                 self.logger.info(f"Processing cell {cell_id}")
                 cell = Cell(cell_id)
@@ -40,13 +42,15 @@ class LoaderSystem:
                     # TODO [SG]: do a break here?
             
             # Check for completed measurements
-            if self.controller.work_queue:
-                completed_slot = self.controller.work_queue.pop(0)
-                cell_id = await self.controller.collect_cell_from_slot(completed_slot)
-                cell = Cell(cell_id)
-                await self.controller.sort_cell(cell)
+            if not self.controller.work_queue:
+                self.logger.debug("No finished measurements in queue")
+                continue
+
+            completed_slot = self.controller.work_queue.pop(0)
+            cell_id = await self.controller.collect_cell_from_slot(completed_slot)
+            cell = Cell(cell_id)
+            await self.controller.sort_cell(cell)
 
-            await asyncio.sleep(0.1)
 
 if __name__ == "__main__":
     loader_system = LoaderSystem()

robot-control/src/robot/controller.py

@@ -115,7 +115,6 @@ class RobotController:
             return
         slot = self.get_next_free_slot()
         if not slot:
-            logger.error("No free slots available")
             return
         await self.insert_cell_to_slot(cell, slot)
 

robot-control/src/robot/movement.py

@@ -21,7 +21,7 @@ class RobotMovement:
         self.grbl.register_position_callback(self._position_callback)
 
     def _position_callback(self, pos: dict):
-        self.current_position = (pos['X'], pos['Y'], pos['Z'])
+        self.current_position = pos
         
     async def set_speed(self, speed_mms: float) -> None:
         if speed_mms <= 0:
@@ -34,7 +34,7 @@ class RobotMovement:
         try:
             self.is_moving = True
             # Send G-code move command
-            await self.grbl.send_gcode([
+            await self.grbl.send_and_wait_gcode([
                 f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{self.speed * 60}'
             ])
             self.is_moving = False
@@ -52,7 +52,7 @@ class RobotMovement:
     async def home(self) -> bool:
         self.is_moving = True
         try:
-            await self.grbl.send_gcode(['$H'])  # GRBL home command
+            await self.grbl.send_and_wait_gcode(['$H'])  # GRBL home command
             self.is_moving = False
             return True
         except Exception as e: