refactor: update GPIO configuration and methods for servo control; improve mag distributor logic

robot_control/config/config.yaml

@@ -114,17 +114,22 @@ gpio:
   debug: false
   pump_pin: 17
   valve_pin: 27
-  probe_pin: 22
-  
-  measure_dir_pin: 11
-  measure_step_pin: 9
-  measure_en_pin: 10
+
+  measure_probe_oping: 22  
+  measure_servo_pin: 11
+  measure_servo_angle_start: 0 
+  measure_servo_angle_end: 45
 
   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_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:

robot_control/src/api/gpio.py

@@ -27,13 +27,13 @@ class GPIOInterface(ABC):
         pass
 
     @abstractmethod
-    def set_servo_angle_smooth(self, pin: int, target_angle: float, speed_deg_per_sec: float) -> None:
+    def set_servo_angle_smooth(self, pin: int, target_angle: float, duration_ms: float) -> None:
         """
-        Move the servo to 'target_angle' at the specified speed (degrees per second).
+        Move the servo to 'target_angle' at the specified speed (duration in ms).
         """
         pass
 
-    def do_step(self, dir_pin: int, step_pin: int, steps: int = 100, step_destep_delay_uslay_s: int = 200, direction: bool = True):
+    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.
         """
@@ -84,11 +84,11 @@ class PiGPIO(GPIOInterface):
         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:
+    def set_servo_angle_smooth(self, pin: int, target_angle_deg: float, duration_ms: float) -> None:
         """
-        Move servo to target_angle at given speed (degrees per second).
+        Move servo to target_angle at given speed (duration in ms).
         """
-        if pin < 0 or pin > 27 or speed_deg_per_sec <= 0:
+        if pin < 0 or pin > 27 or duration_ms <= 0:
             return
         # Read current angle by assuming last set value (no feedback)
         # For simplicity, we store last angle in an instance dict
@@ -103,7 +103,7 @@ class PiGPIO(GPIOInterface):
 
         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
+        delay = duration_ms / int(abs(target_angle_deg-current_angle)) / 1000
         for angle in range(int(current_angle), int(target_angle_deg), step):
             self.set_servo_angle(pin, angle)
             time.sleep(delay)
@@ -157,5 +157,5 @@ class MockGPIO(GPIOInterface):
     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:
+    def set_servo_angle_smooth(self, pin: int, target_angle: float, duration_ms: float) -> None:
         pass

robot_control/src/robot/controller.py

@@ -456,20 +456,21 @@ 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
+            if io_conf.measure_probe_oping < 0: # debug case
                 logger.debug("No probe pin configured, skipping voltage check")
                 return True
-            self.gpio.set_pin(io_conf.probe_pin, 1)
+            self.gpio.set_pin(io_conf.measure_probe_oping, 1)
             await asyncio.sleep(0.1)  # Wait for probe to deploy
             cell_v = await self.i2c.read_channel(1) * VOLTAGE_CONVERSTION_FACTOR  # Measure cell voltage
-            self.gpio.set_pin(io_conf.probe_pin, 0)
+            self.gpio.set_pin(io_conf.measure_probe_oping, 0)
             logger.debug(f"Cell voltage: {cell_v}")
             if self.check_cell_voltage(cell_v, self.next_cell_id):
                 return True # Desired case!
             # Discard cell directly from feeder
             logger.info(f"Cell {self.next_cell_id} voltage({cell_v}) is bad, discarding cell")
-            self.gpio.do_step(io_conf.measure_dir_pin, io_conf.measure_step_pin, HALF_TURN_STEPS, SMOOTH_STEP_DELAY)
-            await asyncio.sleep(1)  # Wait for cell to be ejected
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_end, 1000)
+            await asyncio.sleep(0.25)  # Wait for cell to be ejected
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_start, 250)
             try:
                 self.next_cell_id = ""
                 await self.pick_cell_from_feeder()
@@ -493,8 +494,9 @@ class RobotController:
         cell = self.add_cell(cell_id)
         io_conf = self.config.gpio
         try:
-            self.gpio.do_step(io_conf.measure_dir_pin, io_conf.measure_step_pin, HALF_TURN_STEPS, SMOOTH_STEP_DELAY)
-            await asyncio.sleep(1)  # Wait for cell to be ejected
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_end, 1000)
+            await asyncio.sleep(0.25)  # Wait for cell to be ejected
+            self.gpio.set_servo_angle_smooth(io_conf.measure_servo_pin, io_conf.measure_servo_angle_start, 250)
             await self.pick_cell_from_feeder()
             self.feeder_prepared = False
             await self.insert_cell_to_slot(cell, slot)

robot_control/src/robot/mag_distributor.py

@@ -16,10 +16,13 @@ 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_pin = gpio_conf.mag_dist_rot_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
 
         # 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
@@ -63,7 +66,9 @@ class MagDistributor:
         max_linear_steps = int(self.linear_length_mm * self.steps_per_mm)
         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)
+        await self._home_axis(self.rot_dir_pin, self.rot_step_pin, self.rot_limit_pin, axis='rotational', max_steps=self.full_rot_steps, step_delay=home_step_delay)
         self.curr_pos_mm = 0
+        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):
@@ -89,16 +94,22 @@ class MagDistributor:
         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)
-        linear_dir = True if pos_target > self.curr_pos_mm 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)
+
+        linear_steps = int(abs(pos_target - self.curr_pos_mm) * self.steps_per_mm)
+        linear_dir = True if pos_target > self.curr_pos_mm else False
         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
 
-        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
+        rot_steps = int(abs(rot_target - self.curr_rot_deg) * self.full_rot_steps / 360)
+        rot_dir = True if rot_target > self.curr_rot_deg else False
+        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
 
     def reset_feeder_magazines(self):
         """

robot_control/src/utils/config.py

@@ -115,15 +115,24 @@ class GPIOConfig(BaseModel):
     debug: bool = False
     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)
+
+    # Measure separation
+    measure_probe_oping: int = Field(ge=-1, le=27)
+    measure_servo_pin: int = Field(ge=-1, le=27)
+    measure_servo_angle_start: float = Field(ge=0, le=180)
+    measure_servo_angle_end: float = Field(ge=0, le=180)
+
+    # Mag dist 
     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_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):