ref: improve comments and documentation across modules

main.py

@@ -12,30 +12,39 @@ from robot_control.src.robot.mag_distributor import MagDistributor
 
 class LoaderSystem:
     def __init__(self):
+        # Load configuration
         self.config = ConfigParser().config
         setup_logging(self.config)
         gpio_config = self.config.gpio
+
+        # Initialize GPIO (mock or real based on debug flag)
         if gpio_config.debug:
             self.gpio = MockGPIO()
         else:
             self.gpio = PiGPIO(out_pins=[gpio_config.pump_pin, gpio_config.valve_pin])
 
         self.logger = logging.getLogger(__name__)
+
+        # Initialize camera system
         self.vision = DataMatrixReader(self.config.vision)
         self.logger.info("Initializing LoaderSystem")
         self.vision.initialize()
 
-        # Use mock I2C device if debug is enabled
+        # Initialize I2C (mock or real based on debug flag)
         i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
         self.i2c = I2C(i2c_device_class)
         self.i2c.initialize()
         self.logger.info(f"I2C initialized with {i2c_device_class.__name__}")
 
+        # Initialize pump controller
         self.pump_controller = PumpController(self.config, self.gpio)
 
-        # Initialize MagDistributor
+        # Initialize magazine distributor
         self.mag_distributor = MagDistributor(self.config, self.gpio)
 
+        # Create feeder and defeeder queues used for async handling of cell
+        # Magazine -> MagDist -> Feeder   -> Robot
+        # Magazine <- MagDist <- Defeeder <- Robot
         self.feeder_queue: asyncio.Queue[int] = asyncio.Queue(self.config.feeder.max_capacity)
         self.defeeder_queue: asyncio.Queue[int]  = asyncio.Queue(self.config.defeeder.max_capacity)
 
@@ -51,18 +60,26 @@ class LoaderSystem:
         )
 
     async def run(self):
+        """
+        Main entry point for running the loader system.
+        Starts all main async loops and ensures cleanup on exit.
+        """
         await self.controller.connect()
         try:
             await asyncio.gather(
-                self._loader_loop(),
-                self._poll_i2c_channels(),
-                self._queue_monitor_loop(),
+                self._loader_loop(),         # Main loader orchestration loop
+                self._poll_i2c_channels(),   # Poll I2C sensors
+                self._queue_monitor_loop(),  # Monitor feeder/defeeder queues
             )
         finally:
             self.cleanup()
             self.logger.info("Cleaning up resources...")
 
     async def _poll_i2c_channels(self):
+        """
+        Periodically poll I2C channels for sensor readings.
+        Handles pressure and end-effector state updates.
+        """
         while True:
             try:
                 readings = await self.i2c.read_channels([1, 3, 4])
@@ -70,7 +87,7 @@ class LoaderSystem:
                     self.logger.debug(f"Channel {channel} reading: {value}")
                     if channel == 3:  # Pressure reading
                         self.pump_controller.handle_tank_reading(value)
-                    if channel == 4:
+                    if channel == 4:  # End-effector state
                         state = self.pump_controller.check_endeffector_state(value)
                         self.controller.set_suction_state(state)
             except Exception as e:
@@ -83,7 +100,7 @@ class LoaderSystem:
         Orchestrates cell preparation, slot filling, and measurement processing.
         """
         while True:
-            await asyncio.sleep(0.1)  # avoid busy loop
+            await asyncio.sleep(0.1)  # Avoid busy loop
             while True:
                 # Prepare a cell in the feeder (returns True if a cell is ready)
                 if not await self.controller.prepare_feeder_cell():
@@ -97,6 +114,7 @@ class LoaderSystem:
     async def _queue_monitor_loop(self):
         """
         Periodically checks feeder and defeeder queues and calls placeholder functions.
+        Handles refilling the feeder and processing the defeeder.
         """
         while True:
             # Feeder: If queue below max, trigger refill placeholder
@@ -115,8 +133,10 @@ class LoaderSystem:
                 )
             await asyncio.sleep(2)  # Adjust interval as needed
 
-
     def cleanup(self):
+        """
+        Cleanup hardware resources (GPIO, etc.).
+        """
         self.gpio.cleanup()  # Ensure PumpController cleans up gpio
 
 if __name__ == "__main__":

robot_control/src/api/grbl_handler.py

@@ -3,7 +3,6 @@ import serial_asyncio
 from typing import List, Optional
 import logging
 import datetime
-import collections
 import subprocess
 import os
 

robot_control/src/robot/controller.py

@@ -29,6 +29,7 @@ class Cell:
 
 class RobotController:
     def __init__(self, config: RobotConfig, gpio_handler: GPIOInterface, i2c, vision, pump_controller, feeder_queue: asyncio.Queue[int], defeeder_queue: asyncio.Queue[int]):
+        # Store configuration and hardware interfaces
         self.config = config
         self.cells: dict[int, Cell] = {}
         self.devices = self.config.measurement_devices
@@ -42,7 +43,7 @@ class RobotController:
         self.feeder_queue = feeder_queue
         self.defeeder_queue = defeeder_queue
         
-        # Initialize robot movement
+        # Initialize robot movement and GRBL handler
         self.grbl_handler = GRBLHandler(
             self.config.grbl.port, 
             self.config.grbl.baudrate
@@ -51,14 +52,14 @@ class RobotController:
 
         self.valve_pin = self.config.gpio.valve_pin
         
-        # Initialize with configured values
+        # Calculate total slots and initialize work queue
         self.total_slots = sum(len(device.slots) for device in self.devices)
         self.work_queue: List[MeasurementResult] = []
 
         self.gripper_occupied = False
         self.suction_state = False
         
-        # Initialize MQTT handler
+        # Initialize MQTT handler for measurement devices
         mqtt = self.config.mqtt
         self.mqtt_handler = MQTTHandler(
             broker=mqtt.broker,
@@ -76,6 +77,7 @@ class RobotController:
             )
 
     async def perform_homing(self):
+        """Perform homing sequence for robot axes."""
         await self.movement.perform_homing()
 
     def activate_endeffector(self) -> bool:
@@ -99,9 +101,14 @@ class RobotController:
             raise RuntimeError(f"Failed to deactivate end effector: {str(e)}")
     
     def set_suction_state(self, state:bool):
+        """Set the current suction state (used for feedback from sensors)."""
         self.suction_state = state
 
     async def connect(self):
+        """
+        Connect to GRBL controller and set movement speed.
+        Handles reset attempts if connection fails.
+        """
         try:
             await self.grbl_handler.connect()
         except RuntimeError as e:
@@ -111,21 +118,22 @@ class RobotController:
             except:
                 await self.grbl_handler.reset_usb()
 
-        await self.movement.set_speed(self.config.movement.speed)
-
     async def cleanup(self):
-        """Cleanup resources on shutdown"""
+        """Cleanup resources on shutdown (GRBL, MQTT)."""
         await self.grbl_handler.close()
         self.mqtt_handler.cleanup()
 
     def add_cell(self, cell_id: int) -> Cell:
+        """Add a new cell to the internal cell dictionary."""
         self.cells[cell_id] = Cell(cell_id)
         return self.cells[cell_id]
 
     def get_cell_by_id(self, cell_id: int) -> Optional[Cell]:
+        """Retrieve a cell by its ID."""
         return self.cells.get(cell_id, None)
     
     def get_device_by_id(self, device_id: str) -> Optional[DeviceConfig]:
+        """Retrieve a measurement device by its ID."""
         for device in self.devices:
             if device.id == device_id:
                 return device
@@ -133,6 +141,7 @@ class RobotController:
         return None
     
     def get_slot_by_id(self, device_id: str, slot_id: int) -> Optional[SlotConfig]:
+        """Retrieve a slot by device and slot ID."""
         try:
             device = self.get_device_by_id(device_id)
             if not device:
@@ -143,6 +152,10 @@ class RobotController:
             return None
 
     def get_next_free_slot(self) -> Optional[SlotConfig]:
+        """
+        Find and return the next available (unoccupied) slot.
+        Returns None if all slots are occupied.
+        """
         for device in self.devices:
             for slot in device.slots:
                 if not slot.occupied:
@@ -151,8 +164,12 @@ class RobotController:
                 
         logger.warning("No free slots available")
         return None
-    
+
     async def process_finished_measurement(self):
+        """
+        Process the next finished measurement from the work queue.
+        Picks the cell from the slot and sorts it based on result.
+        """
         if not self.work_queue:
             logger.info("No finished measurements in queue")
             return
@@ -171,7 +188,6 @@ class RobotController:
             return
         await self.pick_cell_from_slot(waiting_slot)
 
-
         if not cell: # Cell not found, create new
             cell = Cell(measurement_result.cell_id, cell_status, capacity=measurement_result.capacity)
             self.cells[measurement_result.cell_id] = cell
@@ -182,6 +198,10 @@ class RobotController:
         await self.sort_cell(cell)
 
     async def pick_cell_from_feeder(self):
+        """
+        Pick a cell from the feeder and update gripper state.
+        Removes one item from the feeder queue.
+        """
         if self.gripper_occupied:
             logger.error("Gripper already occupied")
             return False
@@ -225,6 +245,9 @@ class RobotController:
             raise e
 
     async def insert_cell_to_next_available(self, cell: Cell):
+        """
+        Insert a cell into the next available slot.
+        """
         if not self.gripper_occupied:
             logger.error("Gripper not occupied")
             return
@@ -234,6 +257,9 @@ class RobotController:
         await self.insert_cell_to_slot(cell, slot)
 
     async def insert_cell_to_slot(self, cell: Cell, slot: SlotConfig):
+        """
+        Insert a cell into a specific slot and start measurement.
+        """
         logger.info(f"Inserting cell {cell.id} to {slot}...")
         if slot.occupied:
             raise RuntimeError(f"{slot} is already occupied")
@@ -276,6 +302,9 @@ class RobotController:
         return True
 
     async def pick_cell_from_slot(self, slot: SlotConfig):
+        """
+        Pick a cell from a given slot and update gripper state.
+        """
         if self.gripper_occupied:
             logger.error("Gripper already occupied")
             return None
@@ -315,6 +344,9 @@ class RobotController:
             raise e
 
     async def sort_cell(self, cell: Cell):
+        """
+        Sort a cell into the appropriate magazine based on its capacity/grade.
+        """
         if not self.gripper_occupied:
             logger.error("Gripper not occupied")
             return False
@@ -333,6 +365,10 @@ class RobotController:
         return False
     
     async def dropoff_cell(self, defeeder_mag_idx: Optional[int] = None):
+        """
+        Drop off a cell into the specified defeeder magazine.
+        Adds the magazine index to the defeeder queue for async handling.
+        """
         if not self.gripper_occupied:
             logger.error("Cannot drop off: gripper not occupied")
             return
@@ -367,6 +403,10 @@ class RobotController:
         logger.info(f"Cell dropped off at defeeder")
 
     def check_cell_voltage(self, voltage, cell_id_str = ""):
+        """
+        Check if the cell voltage is within acceptable range.
+        Returns True if voltage is good, False otherwise.
+        """
         if voltage < abs(self.config.feeder.min_voltage):
             logger.info(f"Cell {cell_id_str} voltage too low, discarding cell")
             return False

robot_control/src/robot/mag_distributor.py

@@ -48,7 +48,9 @@ class MagDistributor:
         self.logger = logging.getLogger(__name__)
 
     def _speed_to_step_delay(self, speed_mmmin: int):
-        """Convert speed in mm/min to step delay in ms for do_step."""
+        """
+        Convert speed in mm/min to step delay in ms for do_step.
+        """
         steps_per_s = speed_mmmin * self.steps_per_mm / 60.0  # Convert mm/min to steps/s
         if steps_per_s == 0:
             return 1000  # fallback to slow
@@ -56,7 +58,9 @@ class MagDistributor:
         return int(step_delay_s * 1000)  # ms
 
     async def home(self):
-        """Home both axes using endstops."""
+        """
+        Home both axes using endstops.
+        """
         self.logger.info("Homing linear axis...")
         max_linear_steps = int(self.linear_length_mm * self.steps_per_mm)
         home_step_delay = self._speed_to_step_delay(self.home_speed_mmmin)
@@ -68,6 +72,9 @@ class MagDistributor:
         self.logger.info("Mag distributor homed successfully.")
 
     async def _home_axis(self, dir_pin, step_pin, endstop_pin, axis='linear', max_steps = 10000, step_delay=200):
+        """
+        Home a single axis by moving until the endstop is triggered.
+        """
         if self.debug:
             return
         # Move in negative direction until endstop is triggered
@@ -81,6 +88,9 @@ class MagDistributor:
         self.logger.info(f"{axis} axis homed.")
 
     def move_mag_distributor_at_pos(self, pos_target, rot_target, step_delay=None):
+        """
+        Move the magazine distributor to the specified linear and rotational position.
+        """
         if self.debug:
             return
         self.logger.info(f"Moving mag distributor to linear: {pos_target} mm, rot: {rot_target} deg")
@@ -99,7 +109,9 @@ class MagDistributor:
             self.curr_rot_deg = rot_target
 
     def reset_empty_magazines(self):
-        """Reset the set of magazines already checked for picking."""
+        """
+        Reset the set of magazines already checked for picking.
+        """
         self.empty_magazines.clear()
 
     def mag_to_feeder(self, magazine_id: Optional[int] = None):

robot_control/src/robot/pump_controller.py

@@ -27,9 +27,10 @@ class PumpController:
     def handle_tank_reading(self, pressure_bar):
         current_time = time.time()
 
+        # Check if pump is should be kept off due to watchdog timeout
         if self.watchdog_trigger_time and \
             (current_time - self.watchdog_trigger_time < self.pump_watchdog_timeout):
-            logger.debug("Pump keeps being deactivated due to watchdog timeout")
+            logger.debug(f"Pump keeps being deactivated due to watchdog timeout ({self.pump_watchdog_timeout}s)")
             self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
             self.last_activated_time = None
@@ -38,7 +39,7 @@ class PumpController:
         # Check if pump needs to be deactivated due to watchdog timeout
         if self.pump_active and self.last_activated_time and \
             (current_time - self.last_activated_time > self.max_pump_time_s):
-            logger.warning("Pump deactivated due to watchdog timeout")
+            logger.warning(f"Pump deactivated due to watchdog timeout ({self.max_pump_time_s}s)")
             self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
             self.watchdog_trigger_time = current_time
@@ -47,7 +48,7 @@ class PumpController:
 
         # Activate pump if pressure is above the threshold (lower value is more vacuum)
         if not self.pump_active and pressure_bar > self.pressure_threshold_on:
-            logger.info("Activating pump due to pressure threshold")
+            logger.info(f"Activating pump due to pressure threshold: {pressure_bar} (reading) > {self.pressure_threshold_on}")
             self.gpio.set_pin(self.pump_pin, 1)  # Turn on pump
             self.pump_active = True
             self.watchdog_trigger_time = None
@@ -55,7 +56,7 @@ class PumpController:
 
         # Deactivate pump if pressure is below the threshold
         elif self.pump_active and pressure_bar < self.pressure_threshold_off:
-            logger.info("Deactivating pump due to pressure threshold")
+            logger.info(f"Deactivating pump due to pressure threshold: {pressure_bar} (reading) < {self.pressure_threshold_off}")
             self.gpio.set_pin(self.pump_pin, 0)  # Turn off pump
             self.pump_active = False
             self.watchdog_trigger_time = None

robot_control/src/utils/config.py

@@ -3,6 +3,12 @@ from typing import List, Dict, Tuple, Optional
 from pydantic import BaseModel, Field, model_validator, field_validator
 import yaml
 
+# This module defines the configuration schema for the robot control system.
+# It uses Pydantic models to validate and structure all configuration options,
+# including hardware pin assignments, device positions, magazine layouts, and
+# operational parameters. The configuration is loaded from a YAML file and
+# provides a single source of truth for all system components.
+
 class SlotConfig(BaseModel):
     position: Tuple[float, float, float]
     occupied: bool = False

robot_control/src/utils/logging.py

@@ -3,7 +3,7 @@ from logging.handlers import RotatingFileHandler
 import os
 from .config import RobotConfig
 
-THROTTLING_FILTER_ENABLED = False
+THROTTLING_FILTER_ENABLED = False  # Currently disabled because its not quite working as expected.
 
 # Usage:
 # In main.py, after loading config:
@@ -59,6 +59,10 @@ def setup_logging(config: RobotConfig):
 
 
 import time
+# ThrottlingFilter is a logging filter that suppresses repeated log messages.
+# When enabled, it allows a message to be logged, then suppresses further identical messages
+# for an increasing interval (throttling), up to a maximum. This helps prevent log spam
+# from rapidly repeating errors or warnings, while still allowing occasional updates.
 class ThrottlingFilter(logging.Filter):
     def __init__(self, name='', initial_throttle_interval=1.0, max_throttle_interval=120.0, 
                  throttle_multiplier=2.0, reset_after=120.0):