refactor GRBLHandler and MQTTHandler; improve connection handling, logging, and measurement processing; update cell_id type in MeasurementResult

.gitignore

@@ -5,4 +5,4 @@ robot-control/src/robot_control.egg-info/
 __pycache__
 .coverage
 .python-version
-logs/robot.log
+logs/robot.log*

robot-control/src/api/grbl_handler.py

@@ -3,6 +3,7 @@ import serial_asyncio
 from typing import List
 from utils.logging import LoggerSingleton
 import datetime
+import collections
 
 MOVE_RATE = 50
 APPROACHE_RATE = 10 
@@ -29,23 +30,23 @@ class GRBLHandler:
             return
 
         logger.info("Connecting to GRBL...")
-        try:
-            self.reader, self.writer = await serial_asyncio.open_serial_connection(
-                url = self.port,
-                baudrate = self.baudrate
-            )
-            for _ in range(3):
-                await self._process_response()
-            logger.info("GRBL connected.")
-            # 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
+        self.reader, self.writer = await serial_asyncio.open_serial_connection(
+            url = self.port,
+            baudrate = self.baudrate
+        )
+        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):
+            raise RuntimeError("Failed to connect to GRBL")
+        
+        logger.info("GRBL connected.")
+        # 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
 
     async def send_gcode(self, commands: List[str]):
         """Send GCODE commands"""
@@ -60,12 +61,15 @@ class GRBLHandler:
                 self.writer.write(f"{cmd}\n".encode())
                 await self.writer.drain()
                 await self._process_response()
+        except RuntimeError as e:
+            logger.error(f"Failed to send G-Code commands: {str(e)}")
         finally:
             self.controller_active.clear()
 
     async def wait_until_idle(self, timeout_s):
         """Wait until GRBL reports idle status"""
         if self.debug:
+            asyncio.sleep(1)
             return
             
         start = datetime.datetime.now()
@@ -95,7 +99,7 @@ class GRBLHandler:
         """Process GRBL responses"""
         if self.reader:
             try:
-                response = await asyncio.wait_for(self.reader.readline(), timeout=2.0)  # 2 second timeout
+                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}")
                 
@@ -112,10 +116,8 @@ class GRBLHandler:
                 return decoded
             except asyncio.TimeoutError as e:
                 logger.warning("Timeout waiting for GRBL response")
-                raise e
             except Exception as e:
-                logger.error(f"Failed to parse response: {e}")
-                raise e
+                raise RuntimeError(f"Failed to process response: {e}")
 
     def register_position_callback(self, callback):
         """Register callback for position updates

robot-control/src/api/mqtt_handler.py

@@ -13,7 +13,7 @@ class Device:
 
 class MeasurementResult(BaseModel):
     device_id: str
-    cell_id: str
+    cell_id: int
     slot: int
     capacity: float
     status: str
@@ -39,7 +39,7 @@ class MQTTHandler:
         """Register a new device to handle"""
         self.devices.append(device)
         self.measurement_callbacks[device.device_id] = {}
-        
+
     def _subscribe_device_topics(self, device_id: str):
         """Subscribe to all topics for a specific device"""
         topics = [
@@ -68,7 +68,7 @@ class MQTTHandler:
 
             if topic.startswith("measurement_done/"):
                 result = MeasurementResult(**payload)
-                logger.info(f"Measurement complete for device {device_id}, slot {result.slot}")
+                logger.info(f"Measurement complete {result}")
                 if device_id in self.measurement_callbacks and result.slot in self.measurement_callbacks[device_id]:
                     self.measurement_callbacks[device_id][result.slot](result)
                     

robot-control/src/main.py

@@ -40,15 +40,8 @@ class LoaderSystem:
                     self.logger.error(f"Failed to process cell {cell_id}: {str(e)}")
                     break
             
-            # Check for completed measurements
-            if not self.controller.work_queue:
-                self.logger.warning("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)
+            # Check for completed measurements and sort cell
+            self.controller.process_finished_measurement()
 
 
 if __name__ == "__main__":

robot-control/src/robot/controller.py

@@ -88,6 +88,15 @@ class RobotController:
                 
         logger.warning("No free slots available")
         return None
+    
+    def process_finished_measurement(self):
+        if not self.work_queue:
+            logger.warning("No finished measurements in queue")
+            return
+        completed_slot = self.work_queue.pop(0)
+        cell_id = self.collect_cell_from_slot(completed_slot)
+        cell = Cell(cell_id)
+        self.sort_cell(cell)
 
     async def pick_cell_from_feeder(self):
         if self.gripper_occupied:

robot-control/src/utils/logging.py

@@ -5,6 +5,8 @@ from typing import Optional
 from utils.config import ConfigParser
 import time
 
+THROTTLING_FILTER_ENABLED = False
+
 class LoggerSingleton:
     _instance: Optional[logging.Logger] = None
 
@@ -30,13 +32,15 @@ class LoggerSingleton:
             backupCount=log_config['backup_count']
         )
         file_formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
-        file_handler.addFilter(ThrottlingFilter())
+        if THROTTLING_FILTER_ENABLED:
+            file_handler.addFilter(ThrottlingFilter())
         file_handler.setFormatter(file_formatter)
         logger.addHandler(file_handler)
 
         if log_config['console_output']:
             console_handler = logging.StreamHandler()
-            console_handler.addFilter(ThrottlingFilter())
+            if THROTTLING_FILTER_ENABLED:
+                console_handler.addFilter(ThrottlingFilter())
             console_formatter = logging.Formatter('%(levelname)s: %(message)s')
             console_handler.setFormatter(console_formatter)
             logger.addHandler(console_handler)