feat: add servo control methods to PiGPIO; refactor MagDistributor to use servo for rotation; update config

robot_control/config/config.yaml

@@ -124,10 +124,7 @@ gpio:
   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_rot_pin: -1
   mag_dist_sensor_pin: -1
 
 i2c:

robot_control/src/api/gpio.py

@@ -19,7 +19,24 @@ class GPIOInterface(ABC):
     def get_pin(self, pin: int) -> int:
         pass
 
+    @abstractmethod
+    def set_servo_angle(self, pin: int, angle: float) -> None:
+        """
+        Set the servo connected to 'pin' to the specified angle.
+        """
+        pass
+
+    @abstractmethod
+    def set_servo_angle_smooth(self, pin: int, target_angle: float, speed_deg_per_sec: float) -> None:
+        """
+        Move the servo to 'target_angle' at the specified speed (degrees per second).
+        """
+        pass
+
     def do_step(self, dir_pin: int, step_pin: int, steps: int = 100, step_destep_delay_uslay_s: int = 200, direction: bool = True):
+        """
+        Perform a step operation on a stepper motor.
+        """
         pass
     
     def cleanup(self):
@@ -53,7 +70,55 @@ class PiGPIO(GPIOInterface):
             self.in_pins.append(pin)
         return self.pi.read(pin)  
 
+    def set_servo_angle(self, pin: int, angle_deg: float) -> None:
+        """
+        Set servo to a specific angle (0-180 degrees).
+        MG90 typical pulse width: 0 deg = 500us, 180 deg = 2500us.
+        """
+        if pin not in self.out_pins:
+            self.pi.set_mode(pin, pigpio.OUTPUT)
+            self.out_pins.append(pin)
+        # Clamp angle
+        angle_deg = max(0, min(180, angle_deg))
+        # Map angle to pulse width
+        pulsewidth = int(500 + (angle_deg / 180.0) * 2000)
+        self.pi.set_servo_pulsewidth(pin, pulsewidth)
+
+    def set_servo_angle_smooth(self, pin: int, target_angle_deg: float, speed_deg_per_sec: float) -> None:
+        """
+        Move servo to target_angle at given speed (degrees per second).
+        """
+        if pin < 0 or pin > 27 or speed_deg_per_sec <= 0:
+            return
+        # Read current angle by assuming last set value (no feedback)
+        # For simplicity, we store last angle in an instance dict
+        if not hasattr(self, "_servo_angles"):
+            self._servo_angles:dict[int, float] = {}
+
+        if pin not in self._servo_angles:
+            self.set_servo_angle(pin, target_angle_deg)
+            return
+
+        current_angle = self._servo_angles[pin]
+
+        target_angle_deg = max(0, min(180, target_angle_deg))
+        step = 1 if target_angle_deg > current_angle else -1
+        delay = 1.0 / speed_deg_per_sec
+        for angle in range(int(current_angle), int(target_angle_deg), step):
+            self.set_servo_angle(pin, angle)
+            time.sleep(delay)
+        self.set_servo_angle(pin, target_angle_deg)
+        self._servo_angles[pin] = target_angle_deg
+
     def do_step(self, dir_pin: int, step_pin: int, steps: int = 100, step_delay_us: int = 200, direction: bool = True):
+        """
+        Perform a step operation on a stepper motor.
+        dir_pin: Direction pin
+        step_pin: Step pin to trigger steps
+        steps: Number of steps to perform
+        step_delay_us: Delay between steps in microseconds
+        direction: True for forward, False for reverse
+        """
         # Set direction
         self.set_pin(dir_pin, 1 if direction else 0)
 
@@ -70,7 +135,7 @@ class PiGPIO(GPIOInterface):
         if wave_id >= 0:
             self.pi.wave_send_once(wave_id)
             while self.pi.wave_tx_busy():
-                time.sleep(0.01)
+                time.sleep(0.1)
 
             self.pi.wave_delete(wave_id)
 
@@ -87,4 +152,10 @@ class MockGPIO(GPIOInterface):
         pass
 
     def get_pin(self, pin: int) -> int:      
-        return 0 
+        return 0
+    
+    def set_servo_angle(self, pin: int, angle: float) -> None:
+        pass
+
+    def set_servo_angle_smooth(self, pin: int, target_angle: float, speed_deg_per_sec: float) -> None:
+        pass

robot_control/src/robot/mag_distributor.py

@@ -1,5 +1,6 @@
 import logging
 import asyncio
+import time
 from robot_control.src.utils.config import RobotConfig, DefeederMagazineConfig
 from robot_control.src.api.gpio import GPIOInterface
 from typing import Optional
@@ -15,13 +16,10 @@ class MagDistributor:
         self.linear_dir_pin = gpio_conf.mag_dist_pos_dir_pin
         self.linear_step_pin = gpio_conf.mag_dist_pos_step_pin
         # self.linear_en_pin = gpio_conf.mag_dist_pos_en_pin
-        self.rot_dir_pin = gpio_conf.mag_dist_rot_dir_pin
-        self.rot_step_pin = gpio_conf.mag_dist_rot_step_pin
-        # self.rot_en_pin = gpio_conf.mag_dist_rot_en_pin
+        self.rot_pin = gpio_conf.mag_dist_rot_pin
 
         # Endstop pins
         self.linear_limit_pin = gpio_conf.mag_dist_pos_limit_pin
-        self.rot_limit_pin = gpio_conf.mag_dist_rot_limit_pin
 
         # Cell pick sensor pin
         self.mag_dist_sensor_pin = gpio_conf.mag_dist_sensor_pin  # <-- Add this to your config
@@ -66,9 +64,6 @@ class MagDistributor:
         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...")
-        await self._home_axis(self.rot_dir_pin, self.rot_step_pin, self.rot_limit_pin, axis='rot', max_steps=self.full_rot_steps, step_delay=home_step_delay)
-        self.curr_rot_deg = 0
         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):
@@ -95,18 +90,15 @@ class MagDistributor:
             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)
         linear_dir = True if pos_target > self.curr_pos_mm else False
-        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_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
-        if rot_steps > 0:
-            self.gpio.do_step(self.rot_dir_pin, self.rot_step_pin, rot_steps, step_delay, rot_dir)
-            self.curr_rot_deg = rot_target
+
+        self.gpio.set_servo_angle_smooth(self.rot_pin, rot_target, self.max_speed_mmmin * 0.03183)  # Convert mm/min to deg/s with 3cm radius
 
     def reset_feeder_magazines(self):
         """
@@ -137,11 +129,17 @@ 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:
             pos = magazines[mag_id].mag_pos
+            self.move_mag_distributor_at_pos(pos.pos_mm, 0)
             self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
+            time.sleep(1)
             # Check cell pick sensor
             if self.mag_dist_sensor_pin and self.gpio.get_pin(self.mag_dist_sensor_pin):
+                self.move_mag_distributor_at_pos(pos.pos_mm, 0)
                 pos = self.config.feeder.mag_pos
+                self.move_mag_distributor_at_pos(pos.pos_mm, 0)
                 self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
+                time.sleep(1)
+                self.move_mag_distributor_at_pos(pos.pos_mm, 0)
                 self.logger.info(f"Cell successfully picked from magazine {mag_id} and deposited to feeder.")
                 self.current_magazine = mag_id  # update current
                 return
@@ -164,7 +162,11 @@ class MagDistributor:
 
         # Move to defeeder position
         pos = self.config.defeeder.mag_pos
+        self.move_mag_distributor_at_pos(pos.pos_mm, 0)
         self.move_mag_distributor_at_pos(pos.pos_mm, pos.rot_deg)
+        time.sleep(1)
+        self.move_mag_distributor_at_pos(pos.pos_mm, 0)
+
 
         # Optionally check for cell presence at defeeder (if sensor available)
         if self.mag_dist_sensor_pin and not self.gpio.get_pin(self.mag_dist_sensor_pin):
@@ -172,7 +174,10 @@ class MagDistributor:
             return
 
         # Move to target magazine position
+        self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, 0)
         self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, magazine.mag_pos.rot_deg)
+        time.sleep(1)
+        self.move_mag_distributor_at_pos(magazine.mag_pos.pos_mm, 0)
 
         # Optionally check for successful placement (if sensor logic applies)
         self.logger.info(f"Cell collected from defeeder and moved to magazine {magazine.name}.")

robot_control/src/utils/config.py

@@ -123,10 +123,7 @@ class GPIOConfig(BaseModel):
     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_rot_pin: int = Field(ge=-1, le=27)
     mag_dist_sensor_pin: int = Field(ge=-1, le=27)
 
 class I2CConfig(BaseModel):