feat: update movement and configuration to use speed in mm/min; refactor related classes and tests

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

@@ -9,10 +9,6 @@ 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

@@ -246,7 +246,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)
@@ -284,7 +285,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)

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,9 +47,9 @@ 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
@@ -59,7 +59,7 @@ class MagDistributor:
         """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...")
@@ -90,7 +90,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

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/utils/config.py

@@ -51,8 +51,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 +123,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):

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
         )