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__":

playgrounds/movement_playground.py

@@ -17,7 +17,7 @@ Can also use GPIO pins to control pump and valve.
 # grbl_config = GRBLConfig(port="debug", baudrate=115200)
 grbl_config = GRBLConfig(port="/dev/ttyUSB0", baudrate=115200)
 test_config = MovementConfig(
-        speed=1000,
+        speed_mmmin=1000,
         safe_height=10
     )
 

robot_control/config/config.yaml

@@ -72,8 +72,8 @@ defeeder_magazines:
 
 mag_distributor:
     debug: True
-    max_speed_mms: 100
-    home_speed_mms: 100
+    max_speed_mmmin: 100
+    home_speed_mmmin: 100
     length_mm: 500
 
 mqtt:
@@ -127,8 +127,7 @@ i2c:
   debug: false
 
 movement:
-  speed: 400.0
-  acceleration: 20.0
+  speed_mmmin: 400
   safe_height: 0.0
 
 logging:

robot_control/src/api/grbl_handler.py

@@ -3,16 +3,11 @@ import serial_asyncio
 from typing import List, Optional
 import logging
 import datetime
-import collections
 import subprocess
 import os
 
 logger = logging.getLogger(__name__)
 
-MOVE_RATE = 50
-APPROACHE_RATE = 10 
-CELL_CIRCLE_RADIUS = 0.35
-
 HOMING_TIMEOUT_S = 80
 
 class GRBLHandler:

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")
@@ -246,7 +272,8 @@ class RobotController:
         slot_device = self.get_device_by_id(slot.device_id)
         if not slot_device:
             raise RuntimeError(f"Device {slot.device_id} not found!")
-        pos = [sum(el) for el in zip(slot.position, slot_device.position)]
+        pos = tuple(a + b for a, b in zip(slot.position, slot_device.position))
+        assert(len(pos) == 3), "Position must be a 3-tuple (x, y, z)"
         safe_pos = (pos[0], pos[1], self.config.movement.safe_height)
         logger.info(f"Moving to slot position (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
@@ -275,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
@@ -284,7 +314,8 @@ class RobotController:
         slot_device = self.get_device_by_id(slot.device_id)
         if not slot_device:
             raise RuntimeError(f"Device {slot.device_id} not found")
-        pos = [sum(el) for el in zip(slot.position, slot_device.position)]
+        pos = tuple(a + b for a, b in zip(slot.position, slot_device.position))
+        assert(len(pos) == 3), "Position must be a 3-tuple (x, y, z)"
         safe_pos = (pos[0], pos[1], self.config.movement.safe_height)
         logger.info(f"Moving to slot position (safe) {safe_pos}...")
         await self.movement.move_to_position(*safe_pos)
@@ -313,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
@@ -331,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
@@ -365,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

@@ -29,9 +29,9 @@ class MagDistributor:
         # Max travel (mm or steps, as appropriate)
         self.full_rot_steps = 3200  # Assuming 3200 steps for a full rotation
         self.linear_length_mm = config.mag_distributor.length_mm
-        self.steps_per_mm = self.full_rot_steps / 40 # Assuming 40mm per rotation
-        self.max_speed_mms = config.mag_distributor.max_speed_mms  # mm/s
-        self.home_speed_mms = config.mag_distributor.home_speed_mms  # mm/s
+        self.steps_per_mm = int(self.full_rot_steps / 40) # Assuming 40mm per rotation
+        self.max_speed_mmmin = config.mag_distributor.max_speed_mmmin  # mm/min
+        self.home_speed_mmmin = config.mag_distributor.home_speed_mmmin  # mm/min
 
         self.debug = config.mag_distributor.debug
 
@@ -47,19 +47,23 @@ class MagDistributor:
 
         self.logger = logging.getLogger(__name__)
 
-    def _speed_to_step_delay(self, speed_mm_s):
-        """Convert speed in mm/s to step delay in ms for do_step."""
-        steps_per_s = speed_mm_s * self.steps_per_mm
+    def _speed_to_step_delay(self, speed_mmmin: int):
+        """
+        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
         step_delay_s = 1.0 / steps_per_s
         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_mms)
+        home_step_delay = self._speed_to_step_delay(self.home_speed_mmmin)
         await self._home_axis(self.linear_dir_pin, self.linear_step_pin, self.linear_limit_pin, axis='linear', max_steps=max_linear_steps, step_delay=home_step_delay)
         self.curr_pos_mm = 0
         self.logger.info("Homing rotational axis...")
@@ -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")
@@ -90,7 +100,7 @@ class MagDistributor:
         rot_dir = True if rot_target > self.curr_rot_deg else False
         # Use max speed for normal moves if not specified
         if step_delay is None:
-            step_delay = self._speed_to_step_delay(self.max_speed_mms)
+            step_delay = self._speed_to_step_delay(self.max_speed_mmmin)
         if linear_steps > 0:
             self.gpio.do_step(self.linear_dir_pin, self.linear_step_pin, linear_steps, step_delay, linear_dir)
             self.curr_pos_mm = pos_target
@@ -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/movement.py

@@ -18,7 +18,7 @@ class RobotMovement:
         """    
 
         # Setting configs
-        self.speed: float = config.speed
+        self.default_speed_mmmin: float = config.speed_mmmin
 
         # Basic variables for robot movement
         self.home_position: tuple[float, float, float] = (0.0, 0.0, 0.0)
@@ -38,22 +38,22 @@ class RobotMovement:
         """
         self.current_position = pos
         
-    async def set_speed(self, speed_mms: float) -> None:
+    async def set_speed(self, speed_mmmin: int) -> None:
         """Set the movement speed of the robot.
 
         Args:
-            speed_mms (float): Speed in millimeters per second
+            speed_mmmin (float): Speed in mm/min
 
         Raises:
             ValueError: If speed is not positive
         """
-        if speed_mms <= 0:
-            raise ValueError("Speed must be positive")
-        self.speed = speed_mms
+        if speed_mmmin <= 0:
+            raise ValueError("Default speed must be positive")
+        self.default_speed_mmmin = speed_mmmin
         # Set GRBL feed rate
-        await self.grbl.send_gcode([f'F{speed_mms * 60}'])  # Convert mm/s to mm/min
+        await self.grbl.send_gcode([f'F{speed_mmmin}'])
 
-    async def move_to_position(self, x: float, y: float, z: float) -> tuple[float, float, float]:
+    async def move_to_position(self, x: float, y: float, z: float, speed_mmmin: int = 0) -> tuple[float, float, float]:
         """Move the robot to an absolute position.
 
         Args:
@@ -71,8 +71,12 @@ class RobotMovement:
             self.is_moving = True
             logger.debug(f"Moving to position X:{x:.3f} Y:{y:.3f} Z:{z:.3f}")
             # Send G-code move command
+            if speed_mmmin <= 0:
+                move_command = f'G0 X{x:.3f} Y{y:.3f} Z{z:.3f}'
+            else:
+                move_command = f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{speed_mmmin or self.default_speed_mmmin}'
             await self.grbl.send_and_wait_gcode(
-                [f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{self.speed * 60}'],
+                [move_command],
                 position=self.current_position
             )
             if self.grbl.debug:

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
@@ -51,8 +57,8 @@ class DefeederConfig(BaseModel):
 
 class MagDistributorConfig(BaseModel):
     debug: Optional[bool] = False
-    max_speed_mms: float = Field(default=100.0, ge=0.0)
-    home_speed_mms: float = Field(default=50.0, ge=0.0)
+    max_speed_mmmin: int = Field(default=100, ge=0)
+    home_speed_mmmin: int = Field(default=50, ge=0)
     length_mm: float = Field(default=800.0, ge=0.0)
 
 class DropoffGradeConfig(BaseModel):
@@ -123,8 +129,7 @@ class I2CConfig(BaseModel):
     debug: bool = False
 
 class MovementConfig(BaseModel):
-    speed: float = Field(default=400.0, ge=0.0)
-    acceleration: float = Field(default=20.0, ge=0.0)
+    speed_mmmin: int = Field(default=400, ge=0)
     safe_height: float = Field(default=25.0, ge=0.0)
 
 class LoggingConfig(BaseModel):

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):

tests/test_robot.py

@@ -4,20 +4,33 @@ from robot_control.src.robot.controller import RobotController, Cell, CellStatus
 from robot_control.src.utils.config import ConfigParser
 from robot_control.src.api.grbl_handler import GRBLHandler
 from robot_control.src.api.gpio import MockGPIO
+import asyncio
 
 @pytest.fixture
 def robot_movement():
     config = ConfigParser().config
     grbl = config.grbl
+    grbl.port = "debug"
     grbl_handler = GRBLHandler(grbl.port, grbl.baudrate)
     return RobotMovement(config.movement, grbl_handler)
 
 @pytest.fixture
 def robot_controller():
     config = ConfigParser().config
+    config.grbl.port = "debug"
     config.mqtt.broker = "debug"  # connects to test mqtt broker
     mock_gpio = MockGPIO()
-    controller = RobotController(config, gpio_handler=mock_gpio)
+    feeder_queue = asyncio.Queue(config.feeder.max_capacity)
+    defeeder_queue = asyncio.Queue(config.defeeder.max_capacity)
+    controller = RobotController(
+        config,
+        gpio_handler=mock_gpio,
+        i2c=None,
+        vision=None,
+        pump_controller=None,
+        feeder_queue=feeder_queue,
+        defeeder_queue=defeeder_queue
+    )
     return controller
 
 class TestRobotMovement:
@@ -28,11 +41,12 @@ class TestRobotMovement:
     @pytest.mark.asyncio
     async def test_set_speed(self, robot_movement: RobotMovement):
         await robot_movement.grbl.connect()
-        await robot_movement.set_speed(200.0)
-        assert robot_movement.speed == 200.0
+        speed_mmin = 400
+        await robot_movement.set_speed(speed_mmin)
+        assert robot_movement.default_speed_mmmin == speed_mmin
 
         with pytest.raises(ValueError):
-            await robot_movement.set_speed(-100.0)
+            await robot_movement.set_speed(-400)
 
     # TODO [SG]: Getting current position currently does not work in debug mode
     @pytest.mark.asyncio
@@ -66,8 +80,9 @@ class TestRobotController:
     @pytest.mark.asyncio
     async def test_insert_and_pick_cell(self, robot_controller: RobotController):
         # Test cell insertion
-        cell = Cell(id="test_cell", status=CellStatus.WAITING)
+        cell = Cell(id=1234, status=CellStatus.WAITING)
         slot = robot_controller.get_next_free_slot()
+        assert slot is not None
         robot_controller.gripper_occupied = True
         
         await robot_controller.insert_cell_to_slot(cell, slot)
@@ -83,7 +98,7 @@ class TestRobotController:
     @pytest.mark.asyncio
     async def test_sort_empty_gripper(self, robot_controller: RobotController):
         cell = Cell(
-            id="test_cell",
+            id=1234,
             status=CellStatus.COMPLETED,
             capacity=3000.0
         )
@@ -104,7 +119,7 @@ class TestRobotController:
     async def test_sort_cell_by_capacity(self, robot_controller: RobotController):
         # Test sorting a cell with capacity that matches a grade
         cell = Cell(
-            id="test_cell",
+            id=0,
             status=CellStatus.COMPLETED,
             capacity=3000.0
         )
@@ -114,7 +129,7 @@ class TestRobotController:
 
         # Test sorting a cell with capacity that doesn't match any grade
         low_cap_cell = Cell(
-            id="low_cap_cell",
+            id=1234,
             status=CellStatus.COMPLETED,
             capacity=0.0
         )