feat: update config and logging for magazine distributor; add USB reset functionality; add grbl params at connection

playgrounds/integration_test.py

@@ -22,6 +22,7 @@ async def wait_for_enter():
     event = asyncio.Event()
     
     def input_callback():
+        logging.info("Press Enter to continue...")
         input()  # Wait for Enter
         event.set()
     
@@ -46,6 +47,9 @@ class LoaderSystem:
         # Initialize pump controller
         self.pump_controller = PumpController(self.config, self.gpio)
 
+        i2c_device_class = MCP3428 if not self.config.i2c.debug else MockI2C
+        self.i2c = I2C(i2c_device_class)
+
         # Initialize robot controller with all required arguments
         self.controller = RobotController(
             self.config,
@@ -64,8 +68,6 @@ class LoaderSystem:
         self.test_slot = self.config.measurement_devices[0].slots[3]
 
         # Use mock I2C device if debug is enabled
-        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__}")
 

playgrounds/movement_playground.py

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

robot_control/config/config.yaml

@@ -28,37 +28,54 @@ measurement_devices:
         slot_id: 7
 
 feeder:
-  position: [27.5, 621.5, 47.5]
+  robot_pos: [27.5, 621.5, 47.5]
+  mag_pos: 
+    pos_mm: 0
+    rot_deg: 0
   min_voltage: 2.0
   max_capacity: 10
 
 defeeder:
-  position: [167.5, 630, 40]
+  robot_pos: [167.5, 630, 40]
+  mag_pos: 
+    pos_mm: 0
+    rot_deg: -90
   max_capacity: 10
 
 feeder_magazines:
-  - x_pos: 100
-    rot_deg: 90
+  - mag_pos:
+      pos_mm: 100
+      rot_deg: 90
     capacity: 70
-  - x_pos: 200
-    rot_deg: 90
+  - mag_pos:
+      pos_mm: 200
+      rot_deg: 90
     capacity: 70
-  - x_pos: 300
-    rot_deg: 90
+  - mag_pos:
+      pos_mm: 300
+      rot_deg: 90
     capacity: 70
 
 defeeder_magazines:
-  - x_pos: 200
-    rot_deg: -90
+  - mag_pos:
+      pos_mm: 200
+      rot_deg: -90
     capacity: 70
     health_threshold: 0.8
     name: accepted
-  - x_pos: 100
-    rot_deg: -90
+  - mag_pos:
+      pos_mm: 100
+      rot_deg: -90
     capacity: 70
     health_threshold: 0
     name: rejected
 
+mag_distributor:
+    debug: True
+    max_speed_mms: 100
+    home_speed_mms: 100
+    length_mm: 500
+
 mqtt:
   broker: localhost # or debug
   port: 1883
@@ -96,13 +113,15 @@ gpio:
   measure_step_pin: 9
   measure_en_pin: 10
 
-  dist_pos_dir_pin: 000
-  dist_pos_step_pin: 000
-  dist_pos_en_pin: 000
-
-  dist_rot_dir_pin: 000
-  dist_rot_step_pin: 000
-  dist_rot_en_pin: 000
+  mag_dist_pos_dir_pin: -1
+  mag_dist_pos_step_pin: -1
+  mag_dist_pos_en_pin: -1
+  mag_dist_pos_limit_pin: -1
+  mag_dist_rot_dir_pin: -1
+  mag_dist_rot_step_pin: -1
+  mag_dist_rot_en_pin: -1
+  mag_dist_rot_limit_pin: -1
+  mag_dist_sensor_pin: -1
 
 i2c:
   debug: false

robot_control/config/grbl_config.txt

@@ -0,0 +1,34 @@
+$0=10
+$1=25
+$2=0
+$3=0
+$4=0
+$5=0
+$6=0
+$10=0
+$11=0.010
+$12=0.002
+$13=0
+$20=1
+$21=0
+$22=1
+$23=7
+$24=200.000
+$25=2000.000
+$26=250
+$27=2.500
+$30=1000
+$31=0
+$32=0
+$100=40.000
+$101=40.000
+$102=40.000
+$110=16000.000
+$111=16000.000
+$112=5000.000
+$120=400.000
+$121=400.000
+$122=300.000
+$130=1053.000
+$131=1633.000
+$132=127.500 

robot_control/src/api/gpio.py

@@ -38,12 +38,16 @@ class PiGPIO(GPIOInterface):
             self.pi.set_mode(pin, pigpio.INPUT)
 
     def set_pin(self, pin: int, value: int) -> None:
+        if pin < 0 or pin > 27:
+            return
         if pin not in self.out_pins:
             self.pi.set_mode(pin, pigpio.OUTPUT)
             self.out_pins.append(pin)
         self.pi.write(pin, value)
 
     def get_pin(self, pin: int) -> int:
+        if pin < 0 or pin > 27:
+            return -1
         if pin not in self.in_pins:
             self.pi.set_mode(pin, pigpio.INPUT)
             self.in_pins.append(pin)

robot_control/src/api/grbl_handler.py

@@ -4,6 +4,8 @@ from typing import List, Optional
 import logging
 import datetime
 import collections
+import subprocess
+import os
 
 logger = logging.getLogger(__name__)
 
@@ -31,27 +33,38 @@ class GRBLHandler:
             logger.debug("GRBLHandler is not connected in debug mode")
             return
 
-        logger.info("Connecting to GRBL...")
+        logger.info(f"Connecting to GRBL ({self.port}, {self.baudrate})...")
         try:
             self.reader, self.writer = await serial_asyncio.open_serial_connection(
                 url = self.port,
                 baudrate = self.baudrate
             )            
         except serial_asyncio.serial.SerialException as e:
-            raise serial_asyncio.serial.SerialException(f"Failed to connect to robot: {str(e)}")
+            raise RuntimeError(f"Failed to connect to robot: {str(e)}")
 
         init_response = await self._process_response()
         if not any(isinstance(response, str) and "Grbl" in response for response in init_response):
             raise RuntimeError("Failed to connect to GRBL")
-        
+
         logger.info("GRBL connected.")
 
+        ####################
         # Set GRBL settings:
-        # Enable homing ($22=1)
+        ####################
+        
+        config_path = "robot_control/config/grbl_config.txt"
+        if os.path.exists(config_path):
+            logger.info(f"Loading GRBL config from {config_path}")
+            with open(config_path, "r") as f:
+                config_lines = [line.strip() for line in f if line.strip() and not line.strip().startswith("#")]
+            if config_lines:
+                await self.send_gcode(config_lines)
+        else:
+            logger.warning(f"GRBL config file not found: {config_path}")
+
         # Run homing cycle ($H)
         # Set absolute positioning mode (G90)
-        # Set units to millimeters (G21) 
-        await self.send_gcode(['$22=1']) 
+        # Set units to millimeters (G21)
         await self.send_gcode(['$H'], HOMING_TIMEOUT_S) 
         await self.send_gcode(['G90', 'G21']) 
 
@@ -82,6 +95,29 @@ class GRBLHandler:
             
         await self.connect()  # Reconnect after reset
 
+    async def reset_usb(self):
+        """
+        Power cycle the USB port using uhubctl.
+        Requires uhubctl to be installed and accessible.
+        """
+
+        if self.debug:
+            logger.debug("USB reset skipped in debug mode")
+            return
+
+        cmd = ["uhubctl", "-l", "1-1", "-a", "cycle", "-d", "2"]
+        logger.info(f"Resetting USB with: {' '.join(cmd)}")
+        await asyncio.sleep(0.5)  # Wait for GRBL to reset
+        await self.connect()  # Reconnect after reset
+
+        try:
+            result = subprocess.run(cmd, capture_output=True, text=True, check=True)
+            logger.info(f"USB reset output: {result.stdout.strip()}")
+        except subprocess.CalledProcessError as e:
+            logger.error(f"Failed to reset USB: {e.stderr.strip()}")
+            raise RuntimeError(f"USB reset failed: {e.stderr.strip()}")
+
+
     async def send_gcode(self, commands: List[str], timeout_s = 6):
         """Send GCODE commands"""
         if self.debug:

robot_control/src/robot/controller.py

@@ -13,6 +13,7 @@ logger = logging.getLogger(__name__)
 
 HALF_TURN_STEPS = 1600  # Number of steps for half a turn
 SMOOTH_STEP_DELAY = 200  # Delay in microseconds for smooth stepping
+VOLTAGE_CONVERSTION_FACTOR = 5.1455
 
 class CellStatus(Enum):
     WAITING = "WAITING"
@@ -105,7 +106,10 @@ class RobotController:
             await self.grbl_handler.connect()
         except RuntimeError as e:
             logger.info(f"Connection failed. Try to reset...")
-            await self.grbl_handler.reset()
+            try:
+                await self.grbl_handler.reset()
+            except:
+                await self.grbl_handler.reset_usb()
 
         await self.movement.set_speed(self.config.movement.speed)
 
@@ -381,9 +385,12 @@ class RobotController:
             if not self.next_cell_id:
                 logger.debug("No cell detected")
                 return False  # No cell detected
+            if io_conf.probe_pin < 0: # debug case
+                logger.debug("No probe pin configured, skipping voltage check")
+                return True
             self.gpio.set_pin(io_conf.probe_pin, 1)
             await asyncio.sleep(0.1)  # Wait for probe to deploy
-            cell_v = await self.i2c.read_channel(1)  # Measure cell voltage
+            cell_v = await self.i2c.read_channel(1) * VOLTAGE_CONVERSTION_FACTOR  # Measure cell voltage
             self.gpio.set_pin(io_conf.probe_pin, 0)
             logger.debug(f"Cell voltage: {cell_v}")
             if self.check_cell_voltage(cell_v, self.next_cell_id):

robot_control/src/robot/mag_distributor.py

@@ -30,8 +30,10 @@ class MagDistributor:
         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 = config.mag_distributor.max_speed  # mm/s
-        self.home_speed = config.mag_distributor.home_speed  # mm/s
+        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.debug = config.mag_distributor.debug
 
         # Current position tracking (steps or mm/deg)
         self.curr_pos_mm = 0
@@ -57,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)
+        home_step_delay = self._speed_to_step_delay(self.home_speed_mms)
         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...")
@@ -66,6 +68,8 @@ 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):
+        if self.debug:
+            return
         # Move in negative direction until endstop is triggered
         for _ in range(max_steps):
             if self.gpio.get_pin(endstop_pin):
@@ -77,6 +81,8 @@ class MagDistributor:
         self.logger.info(f"{axis} axis homed.")
 
     def move_mag_distributor_at_pos(self, pos_target, rot_target, step_delay=None):
+        if self.debug:
+            return
         self.logger.info(f"Moving mag distributor to linear: {pos_target} mm, rot: {rot_target} deg")
         linear_steps = int(abs(pos_target - self.curr_pos_mm) * self.steps_per_mm)
         rot_steps = int(abs(rot_target - self.curr_rot_deg) / 360 * self.full_rot_steps)
@@ -84,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)
+            step_delay = self._speed_to_step_delay(self.max_speed_mms)
         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
@@ -101,7 +107,7 @@ class MagDistributor:
         Move a cell from a magazine to the feeder.
         If magazine_id is None, use current_magazine and try next if pick fails.
         """
-        magazines = self.config.feeder.magazines
+        magazines = self.config.feeder_magazines
         if magazine_id is not None:
             mags_to_try = [magazine_id]
         else:
@@ -111,15 +117,12 @@ class MagDistributor:
             
         # Check if cell can be picked from magazines, if not add to empty magazines and continue
         for mag_id in mags_to_try:
-            x_pos = magazines[mag_id].x_pos
-            rot_deg = magazines[mag_id].rot_deg
-            self.move_mag_distributor_at_pos(x_pos, rot_deg)
+            pos = magazines[mag_id].mag_pos
+            self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
             # Check cell pick sensor
-            if self.gpio.get_pin(self.mag_dist_sensor_pin):
+            if self.mag_dist_sensor_pin and self.gpio.get_pin(self.mag_dist_sensor_pin):
                 pos = self.config.feeder.mag_pos
-                x_pos = magazines[mag_id].x_pos
-                rot_deg = magazines[mag_id].rot_deg
-                self.move_mag_distributor_at_pos(pos[0], pos[1])
+                self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
                 self.logger.info(f"Cell successfully picked from magazine {mag_id} and deposited to feeder.")
                 self.current_magazine = mag_id  # update current
                 return
@@ -135,22 +138,23 @@ class MagDistributor:
         Includes checks for valid magazine_id and sensor confirmation.
         """
         # Check magazine_id validity
-        magazines = self.config.defeeder.magazines
+        magazines = self.config.defeeder_magazines
         if magazine_id < 0 or magazine_id >= len(magazines):
             self.logger.error(f"Invalid magazine_id: {magazine_id}")
             return
 
         # Move to defeeder position
         pos = self.config.defeeder.mag_pos
-        self.move_mag_distributor_at_pos(pos[0], pos[1])
+        self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
 
         # Optionally check for cell presence at defeeder (if sensor available)
-        if not self.gpio.get_pin(self.mag_dist_sensor_pin):
+        if self.mag_dist_sensor_pin and not self.gpio.get_pin(self.mag_dist_sensor_pin):
             self.logger.warning("No cell detected at defeeder position.")
             return
 
         # Move to target magazine position
-        self.move_mag_distributor_at_pos(magazines[magazine_id].x_pos, magazines[magazine_id].rot_deg)
+        pos = magazines[magazine_id].mag_pos
+        self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
 
         # Optionally check for successful placement (if sensor logic applies)
         self.logger.info(f"Cell collected from defeeder and moved to magazine {magazine_id}.")

robot_control/src/utils/config.py

@@ -26,9 +26,12 @@ class DeviceConfig(BaseModel):
             raise ValueError("Duplicate slot IDs found")
         return v
 
-class MagazineConfig(BaseModel):
-    x_pos: float = Field(default=0.0, ge=0, le=700)
+class MagDistPosition(BaseModel):
+    pos_mm: float = Field(default=0.0, ge=0, le=700)
     rot_deg: float = Field(default=0.0, ge=-180.0, le=180.0)
+
+class MagazineConfig(BaseModel):
+    mag_pos: MagDistPosition
     capacity: int = Field(default=10, ge=0)
 
 class DefeederMagazineConfig(MagazineConfig):
@@ -37,20 +40,19 @@ class DefeederMagazineConfig(MagazineConfig):
 
 class FeederConfig(BaseModel):
     robot_pos: Tuple[float, float, float]
-    mag_pos : Tuple[float, float]
+    mag_pos: MagDistPosition
     min_voltage: float = Field(default=2.0, ge=0.0)
     max_capacity: int = Field(default=10, ge=0)
-    magazines: List[MagazineConfig] = Field(default_factory=list)
 
 class DefeederConfig(BaseModel):
     robot_pos: Tuple[float, float, float]
-    mag_pos : Tuple[float, float]
+    mag_pos: MagDistPosition
     max_capacity: int = Field(default=10, ge=0)
-    magazines: List[DefeederMagazineConfig] = Field(default_factory=list)
 
 class MagDistributorConfig(BaseModel):
-    max_speed: float = Field(default=100.0, ge=0.0)
-    home_speed: float = Field(default=50.0, ge=0.0)
+    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)
     length_mm: float = Field(default=800.0, ge=0.0)
 
 class DropoffGradeConfig(BaseModel):
@@ -101,21 +103,21 @@ class VacuumConfig(BaseModel):
 
 class GPIOConfig(BaseModel):
     debug: bool = False
-    pump_pin: int = Field(ge=0, le=25)
-    valve_pin: int = Field(ge=0, le=25)
-    probe_pin: int = Field(ge=0, le=25)
-    measure_dir_pin: int = Field(ge=0, le=25)
-    measure_step_pin: int = Field(ge=0, le=25)
-    measure_en_pin: int = Field(default=0, ge=0, le=25)
-    mag_dist_pos_dir_pin: int = Field(ge=0, le=25)
-    mag_dist_pos_step_pin: int = Field(ge=0, le=25)
-    mag_dist_pos_en_pin: int = Field(default=0, ge=0, le=25)
-    mag_dist_pos_limit_pin: int = Field(ge=0, le=25)
-    mag_dist_rot_dir_pin: int = Field(ge=0, le=25)
-    mag_dist_rot_step_pin: int = Field(ge=0, le=25)
-    mag_dist_rot_en_pin: int = Field(default=0, ge=0, le=25)
-    mag_dist_rot_limit_pin: int = Field(ge=0, le=25)
-    mag_dist_sensor_pin: int = Field(ge=0, le=25)
+    pump_pin: int = Field(ge=-1, le=27)
+    valve_pin: int = Field(ge=-1, le=27)
+    probe_pin: int = Field(ge=-1, le=27)
+    measure_dir_pin: int = Field(ge=-1, le=27)
+    measure_step_pin: int = Field(ge=-1, le=27)
+    measure_en_pin: int = Field(default=0, ge=-1, le=27)
+    mag_dist_pos_dir_pin: int = Field(ge=-1, le=27)
+    mag_dist_pos_step_pin: int = Field(ge=-1, le=27)
+    mag_dist_pos_en_pin: int = Field(default=0, ge=-1, le=27)
+    mag_dist_pos_limit_pin: int = Field(ge=-1, le=27)
+    mag_dist_rot_dir_pin: int = Field(ge=-1, le=27)
+    mag_dist_rot_step_pin: int = Field(ge=-1, le=27)
+    mag_dist_rot_en_pin: int = Field(default=0, ge=-1, le=27)
+    mag_dist_rot_limit_pin: int = Field(ge=-1, le=27)
+    mag_dist_sensor_pin: int = Field(ge=-1, le=27)
 
 class I2CConfig(BaseModel):
     debug: bool = False

robot_control/src/utils/logging.py

@@ -29,6 +29,12 @@ def setup_logging(config: RobotConfig):
 
     handlers = []
     # File handler
+    class ShortNameFormatter(logging.Formatter):
+        def format(self, record):
+            # Only use the last part of the logger name
+            record.shortname = record.name.split('.')[-1]
+            return super().format(record)
+
     file_handler = RotatingFileHandler(
         log_config.file_path,
         maxBytes=log_config.max_file_size_mb * 1024 * 1024,
@@ -45,7 +51,7 @@ def setup_logging(config: RobotConfig):
         console_handler = logging.StreamHandler()
         if THROTTLING_FILTER_ENABLED:
             console_handler.addFilter(ThrottlingFilter())
-        console_formatter = logging.Formatter('%(levelname)s: %(name)s: %(message)s')
+        console_formatter = ShortNameFormatter('%(levelname)s: %(shortname)s: %(message)s')
         console_handler.setFormatter(console_formatter)
         handlers.append(console_handler)
 

robot_control/src/vision/datamatrix.py

@@ -61,9 +61,8 @@ class DataMatrixReader:
             raise Exception("No cam available")
         
         ret, frame = self.camera.read()
-        if not ret or not frame:
+        if not ret or frame is None or frame.size == 0:
             raise Exception("Failed to capture image")
-        
         if self.bbox:
             x, y, w, h = self.bbox
             frame = frame[y:y+h, x:x+w]