refactor logging; refactor exception handling

robot-control/config/config.yaml

@@ -1,40 +1,40 @@
 measurement_devices:
   - id: "device_1"
-    position: [100.0, 200.0, 50.0]
+    position: [50.0, 50.0, 0.0]
     slots:
-      - position: [10.0, 20.0, 50.0]
+      - position: [10.0, 20.0, 0.0]
         occupied: False
         slot_id: 0
-      - position: [20.0, 20.0, 50.0]
+      - position: [20.0, 20.0, 0.0]
         occupied: False
         slot_id: 1
-      - position: [30.0, 20.0, 50.0]
+      - position: [30.0, 20.0, 0.0]
         occupied: False
         slot_id: 2
 
   - id: "device_2"
-    position: [100.0, 300.0, 50.0]
+    position: [100.0, 50.0, 0.0]
     slots:
-      - position: [10.0, 20.0, 50.0]
+      - position: [10.0, 20.0, 0.0]
         occupied: False
         slot_id: 0
-      - position: [20.0, 20.0, 50.0]
+      - position: [20.0, 20.0, 0.0]
         occupied: False
         slot_id: 1
-      - position: [30.0, 20.0, 50.0]
+      - position: [30.0, 20.0, 0.0]
         occupied: False
         slot_id: 2
 
 feeder:
-  position: [0.0, 0.0, 50.0]
-  approach_position: [0.0, 0.0, 100.0]
+  position: [0.0, 0.0, 0.0]
+  approach_position: [0.0, 0.0, 20.0]
 
 dropoff_grades:
   accepted:
-    position: [500.0, 0.0, 50.0]
+    position: [0.0, 10.0, 0.0]
     capacity_threshold: 0.8
   rejected:
-    position: [500.0, 100.0, 50.0]
+    position: [0.0, 20.0, 0.0]
     capacity_threshold: 0
 
 mqtt:
@@ -46,15 +46,16 @@ mqtt:
 
 grbl:
   port: /dev/ttyACM0 # or debug
+  # port: debug
   baudrate: 115200
 
 system_settings:
-  speed: 100.0
-  acceleration: 500.0
-  safe_height: 20.0
+  speed: 400.0
+  acceleration: 20.0
+  safe_height: 25.0
 
 logging:
-  level: DEBUG
+  level: INFO
   file_path: logs/robot.log
   max_file_size_mb: 10
   backup_count: 5

robot-control/requirements.txt

@@ -1,6 +1,6 @@
 fastapi[all]
 uvicorn
-opencv-python==4.10.0
+# opencv-python==4.10.0
 pylibdmtx==0.1.10
 pydantic==2.10.4
 numpy==2.0.0
@@ -9,4 +9,5 @@ pytest==7.4.2
 pytest-asyncio==0.21.1
 pytest-cov==6.0.0
 httpx==0.25.0
-paho-mqtt<2
+paho-mqtt<2
+pyserial-asyncio>=0.6

robot-control/src/api/grbl_handler.py

@@ -111,12 +111,12 @@ class GRBLHandler:
                         callback(self.current_position)
                 
                 return decoded
-            except asyncio.TimeoutError:
+            except asyncio.TimeoutError as e:
                 logger.warning("Timeout waiting for GRBL response")
-                return None
+                raise e
             except Exception as e:
-                logger.error(f"Failed to parse position: {e}")
-                return None
+                logger.error(f"Failed to parse response: {e}")
+                raise e
 
     def register_position_callback(self, callback):
         """Register callback for position updates

robot-control/src/api/mqtt_handler.py

@@ -39,9 +39,7 @@ class MQTTHandler:
         """Register a new device to handle"""
         self.devices.append(device)
         self.measurement_callbacks[device.device_id] = {}
-        # Subscribe to device specific topics
-        self._subscribe_device_topics(device.device_id)
-
+        
     def _subscribe_device_topics(self, device_id: str):
         """Subscribe to all topics for a specific device"""
         topics = [

robot-control/src/main.py

@@ -15,7 +15,6 @@ class LoaderSystem:
         self.vision.initialize()
 
     async def run(self):
-        self.logger.info("Connecting to robot controller")
         await self.controller.connect()
 
         # Main robot control loop

robot-control/src/robot/controller.py

@@ -78,14 +78,15 @@ class RobotController:
             logger.error("Gripper already occupied")
             return False
             
+        logger.info("Move to save position and pick cell from feeder...")
+        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
         try:
-            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
             await self.movement.activate_endeffector()
             self.gripper_occupied = True
 
@@ -103,7 +104,7 @@ class RobotController:
         for device in self.devices:
             for slot in device.slots:
                 if not slot.occupied:
-                    logger.info(f"Next free slot found at position {slot.position}")
+                    logger.info(f"Next free slot is {slot}")
                     return slot
                 
         logger.error("No free slots available")
@@ -119,26 +120,21 @@ class RobotController:
         await self.insert_cell_to_slot(cell, slot)
 
     async def insert_cell_to_slot(self, cell: Cell, slot: SlotConfig):
+        logger.info(f"Inserting cell {cell.id} to {slot}...")
         if slot.occupied:
-            logger.error(f"Slot at position {slot.position} is already occupied")
-            return False
+            raise RuntimeError(f"{slot} is already occupied")
         if not self.gripper_occupied:
-            logger.error("Gripper not occupied")
-            return False
+            raise RuntimeError("Gripper not occupied")
 
         # Move to slot position
-        try:
-            x, y, z = slot.position
-            # Move to safe height first
-            await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-            # Move down to insertion position
-            await self.movement.move_to_position(x, y, z)
-            # Release cell
-            if not await self.movement.deactivate_endeffector():
-                raise RuntimeError("Failed to release cell")     
-        except Exception as e:
-            logger.error(f"Failed to insert cell: {str(e)}")
-            raise e  
+        x, y, z = slot.position
+        # Move to safe height first
+        await self.movement.move_to_position(x, y, self.system_settings.safe_height)
+        # Move down to insertion position
+        await self.movement.move_to_position(x, y, z)
+        # Release cell
+        if not await self.movement.deactivate_endeffector():
+            raise RuntimeError(f"Failed to release cell {cell.id}")     
             
         slot.occupied = True
         slot.cell_id = cell.id
@@ -156,7 +152,7 @@ class RobotController:
         
         # Move back to safe height
         await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-        logger.info(f"Cell {cell.id} inserted to slot at position {slot.position}")
+        logger.info(f"Cell {cell.id} inserted to {slot}")
         return True
 
     async def collect_cell_from_slot(self, slot: SlotConfig):
@@ -164,14 +160,14 @@ class RobotController:
             logger.error("Gripper already occupied")
             return None
         
+        x, y, z = slot.position
+        # Move to safe height first
+        await self.movement.move_to_position(x, y, self.system_settings.safe_height)
+        # Move down to collection position
+        await self.movement.move_to_position(x, y, z)
+        # Grip cell
         try:
-            x, y, z = slot.position
-            # Move to safe height first
-            await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-            # Move down to collection position
-            await self.movement.move_to_position(x, y, z)
-            # Grip cell
-            if not await self.movement.activate_gripper():
+            if not await self.movement.activate_endeffector():
                 raise RuntimeError("Failed to grip cell")
             self.gripper_occupied = True
             cell_id = slot.cell_id
@@ -180,13 +176,12 @@ class RobotController:
             
             # Move back to safe height
             await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-            logger.info(f"Cell {cell_id} collected from slot at position {slot.position}")
+            logger.info(f"Cell {cell_id} collected from {slot}")
             return cell_id
         except Exception as e:
-            logger.error(f"Failed to collect cell: {str(e)}")
             await self.movement.deactivate_endeffector() # Try to deactivate to avoid stuck gripper
             self.gripper_occupied = False
-            return None
+            raise e
 
     async def sort_cell(self, cell: Cell):
         if not self.gripper_occupied:
@@ -210,19 +205,16 @@ class RobotController:
             logger.error("Cannot drop off: gripper not occupied")
             return
             
-        try:
-            x, y, z = dropoff_grade.position
-            # Move to safe height first
-            await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-            # Move to dropoff position
-            await self.movement.move_to_position(x, y, z)
-            # Release cell
-            if not await self.movement.deactivate_endeffector():
-                raise RuntimeError("Failed to release cell")
-            self.gripper_occupied = False
+        x, y, z = dropoff_grade.position
+        # Move to safe height first
+        await self.movement.move_to_position(x, y, self.system_settings.safe_height)
+        # Move to dropoff position
+        await self.movement.move_to_position(x, y, z)
+        # Release cell
+        if not await self.movement.deactivate_endeffector():
+            raise RuntimeError("Failed to release cell")
+        self.gripper_occupied = False
 
-            # Move back to safe height
-            await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-            logger.info(f"Cell dropped off at grade {dropoff_grade.name}")
-        except Exception as e:
-            logger.error(f"Failed to drop off cell: {str(e)}")
+        # Move back to safe height
+        await self.movement.move_to_position(x, y, self.system_settings.safe_height)
+        logger.info(f"Cell dropped off at grade {dropoff_grade.name}")

robot-control/src/robot/movement.py

@@ -33,6 +33,7 @@ class RobotMovement:
     async def move_to_position(self, x: float, y: float, z: float) -> bool:
         try:
             self.is_moving = True
+            logger.debug(f"Moving to position X:{x:.3f} Y:{y:.3f} Z:{z:.3f}")
             # Send G-code move command
             await self.grbl.send_and_wait_gcode([
                 f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{self.speed * 60}'
@@ -44,6 +45,7 @@ class RobotMovement:
             raise RuntimeError(f"Movement failed: {str(e)}")
 
     async def move_relative(self, dx: float, dy: float, dz: float) -> bool:
+        logger.debug(f"Moving relative by dX:{dx:.3f} dY:{dy:.3f} dZ:{dz:.3f}")
         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
@@ -52,6 +54,7 @@ class RobotMovement:
     async def home(self) -> bool:
         self.is_moving = True
         try:
+            logger.info("Homing machine...")
             await self.grbl.send_and_wait_gcode(['$H'])  # GRBL home command
             self.is_moving = False
             return True
@@ -73,15 +76,18 @@ class RobotMovement:
                 self.pump_speed = max(0, min(1000, speed))
             await self.grbl.send_gcode([f'M3 S{self.pump_speed}'])
             self.endeffector_active = True
+            logger.debug("End effector activated")
             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
+            logger.debug("End effector deactivated")
             return True
         except Exception as e:
             raise RuntimeError(f"Failed to deactivate end effector: {str(e)}")
@@ -94,6 +100,7 @@ class RobotMovement:
         """Emergency stop."""
         self.is_moving = False
         self.endeffector_active = False
+        logger.warning("Emergency stop triggered!")
         await self.grbl.send_gcode(['!', 'M5'])  # Feed hold + stop pump
 
     async def wait_until_idle(self, timeout: Optional[float] = None) -> bool:

robot-control/src/utils/config.py

@@ -11,6 +11,9 @@ class SlotConfig:
     device_id: str
     cell_id: str = None
 
+    def __str__(self) -> str:
+        return f"Slot {self.slot_id}, Device: {self.device_id}, Pos: {self.position}"
+
 @dataclass
 class DeviceConfig:
     id: str