feat: enhance loader system with cell voltage checks and feeder preparation; add motor step control

main.py

@@ -36,6 +36,8 @@ class LoaderSystem:
 
         self.pump_controller = PumpController(self.config, self.gpio)
 
+        self.feeder_prepared = False
+
     async def run(self):
         await self.controller.connect()
         try:
@@ -62,25 +64,78 @@ class LoaderSystem:
                 self.logger.error(f"Error polling I2C channels: {str(e)}")
             await asyncio.sleep(1)  # Poll every second
 
+    def check_cell_voltage(self, voltage, cell_id_str = ""):
+        if voltage < abs(self.config.feeder.min_voltage):
+            self.logger.info(f"Cell {cell_id_str} voltage too low, discarding cell")
+            return False
+        if voltage < 0:
+            self.logger.info(f"Cell {cell_id_str} has wrong polarity, discarding cell")
+            return False
+        
+        self.logger.info(f"Cell {cell_id_str} voltage({voltage}) is good")
+        return True
+
     async def _loader_loop(self):
+        """
+        Main loop for the loader system.
+
+        Checks for free slots and tries to fill them with cells.
+        If no more free slots are available, it checks for completed measurements
+        and sorts the cells accordingly.
+        """
         while True:
             await asyncio.sleep(0.1)  # avoid busy loop
 
             # Check for free slots loop
             while True:
+                cell_id_str = ""
+                # Discard cells until acceptable cell is found
+                while not self.feeder_prepared:
+                    # Check if cell is present in feeder
+                    cell_id_str = self.vision.read_datamatrix()
+                    if not cell_id_str:
+                        self.logger.debug("No cell detected")
+                        break # No cell detected
+                    
+                    # Measure cell voltage with probe
+                    io_conf = self.config.gpio
+                    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
+                    self.gpio.set_pin(io_conf.probe_pin, 0)
+                    self.logger.debug(f"Cell voltage: {cell_v}")
+
+                    if self.check_cell_voltage(cell_v, cell_id_str):
+                        self.feeder_prepared = True
+                        break # Desired case!
+
+                    # Discard cell directly from feeder
+                    self.logger.info(f"Cell {cell_id_str} voltage({cell_v}) is bad, discarding cell")
+                    self.gpio.do_step(io_conf.measure_dir_pin, io_conf.measure_step_pin, 1600, 200) # Exactly half a turn
+                    await asyncio.sleep(1)  # Wait for cell to be ejected
+                    try:
+                        await self.controller.pick_cell_from_feeder()
+                        await self.controller.dropoff_cell()
+                    except Exception as e:
+                        self.logger.error(f"Failed to process cell {cell_id}: {str(e)}")
+                        break
+
                 slot = self.controller.get_next_free_slot()
                 if not slot:
-                    break
-                # Pick and place new cell
-                cell_id_str = self.vision.read_datamatrix()
+                    break # No free slots available
+
                 if not cell_id_str:
-                    self.logger.debug("No cell detected")
                     break
+
+                # Pick and place new cell
                 cell_id = int(cell_id_str)
                 self.logger.info(f"Processing cell {cell_id}")
                 cell = self.controller.add_cell(cell_id)
                 try:
+                    self.gpio.do_step(io_conf.measure_dir_pin, io_conf.measure_step_pin, 1600, 200) # Exactly half a turn
+                    await asyncio.sleep(1)  # Wait for cell to be ejected
                     await self.controller.pick_cell_from_feeder()
+                    self.feeder_prepared = False
                     await self.controller.insert_cell_to_slot(cell, slot)
                 except Exception as e:
                     self.logger.error(f"Failed to process cell {cell_id}: {str(e)}")

robot_control/config/config.yaml

@@ -29,6 +29,7 @@ measurement_devices:
 
 feeder:
   position: [27.5, 621.5, 47.5]
+  min_voltage: 2.0
 
 dropoff_grades:
   - id: accepted
@@ -69,6 +70,10 @@ gpio:
   debug: false
   pump_pin: 17
   valve_pin: 27
+  probe_pin: 22
+  measure_dir_pin: 11
+  measure_step_pin: 9
+  measure_en_pin: 10
 
 i2c:
   debug: false

robot_control/src/api/gpio.py

@@ -1,7 +1,12 @@
 from abc import ABC, abstractmethod
 import pigpio
+import time
 
 class GPIOInterface(ABC):
+    """
+    Interface for handling communication to the GPIO pins.
+    out_pins are set low when program is aborted.
+    """
     def __init__(self, in_pins: list=[], out_pins: list=[]):
         self.out_pins = out_pins
         self.in_pins = in_pins
@@ -13,6 +18,9 @@ class GPIOInterface(ABC):
     @abstractmethod
     def get_pin(self, pin: int) -> int:
         pass
+
+    def do_step(self, dir_pin: int, step_pin: int, steps: int = 100, step_destep_delay_uslay_s: int = 200, direction: bool = True):
+        pass
     
     def cleanup(self):
         pass
@@ -33,8 +41,29 @@ class PiGPIO(GPIOInterface):
         self.pi.write(pin, value)
 
     def get_pin(self, pin: int) -> int:
-        return self.pi.read(pin)
-    
+        return self.pi.read(pin)  
+
+    def do_step(self, dir_pin: int, step_pin: int, steps: int = 100, step_delay_us: int = 200, direction: bool = True):
+        # Set direction
+        self.set_pin(dir_pin, 1 if direction else 0)
+
+        # Create pulse waveform
+        pulses = []
+        for _ in range(steps):
+            pulses.append(pigpio.pulse(1 << step_pin, 0, step_delay_us))   # STEP high
+            pulses.append(pigpio.pulse(0, 1 << step_pin, step_delay_us))   # STEP low
+
+        self.pi.wave_clear()
+        self.pi.wave_add_generic(pulses)
+        wave_id = self.pi.wave_create()
+
+        if wave_id >= 0:
+            self.pi.wave_send_once(wave_id)
+            while self.pi.wave_tx_busy():
+                time.sleep(0.01)
+
+            self.pi.wave_delete(wave_id)
+
     def cleanup(self):
         if self.pi.connected:
             for pin in self.out_pins:

robot_control/src/api/i2c_handler.py

@@ -24,21 +24,22 @@ class I2C:
         assert self.address is not None, "I2C address must be provided"
         self.kwargs = kwargs
         self.device: Optional[I2CDevice] = None
+        self.lock = asyncio.Lock()  # Add lock for thread safety
 
     def initialize(self):
         self.device = self.device_class()
         self.device.initialize(self.address)
         
     async def read_channel(self, channel):
-        if self.device is None:
-            raise RuntimeError("I2C not initialized")
-        self.device.config_channel(channel, **self.kwargs)
-        await asyncio.sleep(0.01)
-        return self.device.read_value()
+        async with self.lock:
+            if self.device is None:
+                raise RuntimeError("I2C not initialized")
+            self.device.config_channel(channel, **self.kwargs)
+            await asyncio.sleep(0.01)
+            return self.device.read_value()
     
     async def read_channels(self, channels):
         self.channels = channels if isinstance(channels, list) else [channels]
-        
         # Needs to be sequential otherwise readings will be mixedup
         return {channel: await self.read_channel(channel) for channel in self.channels}
     

robot_control/src/robot/controller.py

@@ -310,12 +310,19 @@ class RobotController:
                 return True
         logger.error(f"No suitable grade found for cell {cell.id} with capacity {cell.capacity}")
         return False
-
-    async def dropoff_cell(self, dropoff_grade: DropoffGradeConfig):
+    
+    async def dropoff_cell(self, dropoff_grade: Optional[DropoffGradeConfig] = None):
         if not self.gripper_occupied:
             logger.error("Cannot drop off: gripper not occupied")
             return
-            
+
+        # If no dropoff_grade is given, use the last one in the list
+        if dropoff_grade is None:
+            if not self.dropoff_grades:
+                logger.error("No dropoff grades configured")
+                return
+            dropoff_grade = self.dropoff_grades[-1]
+
         pos = dropoff_grade.position
         safe_pos = (pos[0], pos[1], self.config.movement.safe_height)
         logger.info(f"Moving to dropoff (safe) {safe_pos}...")

robot_control/src/utils/config.py

@@ -28,6 +28,7 @@ class DeviceConfig(BaseModel):
 
 class FeederConfig(BaseModel):
     position: Tuple[float, float, float]
+    min_voltage: float = Field(default=2.0, ge=0.0)
 
 class DropoffGradeConfig(BaseModel):
     id: str
@@ -76,8 +77,12 @@ class VacuumConfig(BaseModel):
 
 class GPIOConfig(BaseModel):
     debug: bool = False
-    pump_pin: int = Field(default=17, ge=0)
-    valve_pin: int = Field(default=27, ge=0)
+    pump_pin: int = Field(default=17, ge=0, le=25)
+    valve_pin: int = Field(default=27, ge=0, le=25)
+    probe_pin: int = Field(default=22, ge=0, le=25)
+    measure_dir_pin: int = Field(default=11, ge=0, le=25)
+    measure_step_pin: int = Field(default=9, ge=0, le=25)
+    measure_en_pin: int = Field(default=10, ge=0, le=25)
 
 class I2CConfig(BaseModel):
     debug: bool = False