add: mag_to_feeder_playground, mag_to_feeder_moving_sequence, feat: do_step function with chunking to prevent peer connection lost error

.vscode/launch.json

@@ -25,6 +25,20 @@
             "module": "playgrounds.i2c_playground",
             "console": "integratedTerminal"
         },
+        {
+            "name": "Mag Dist Playground",
+            "type": "debugpy",
+            "request": "launch",
+            "module": "playgrounds.mag_dist_playground",
+            "console": "integratedTerminal"
+        },
+        {
+            "name": "Mag to Feeder Moving Sequence",
+            "type": "debugpy",
+            "request": "launch",
+            "module": "playgrounds.mag_to_feeder_moving_sequence",
+            "console": "integratedTerminal"
+        },        
         {
             "name": "Movement_Playground",
             "type": "debugpy",

playgrounds/mag_dist_playground.py

@@ -10,7 +10,7 @@ def main():
     # You may need to adjust these to your environment
     config = ConfigParser().config
     gpio_config = config.gpio
-    gpio_config.mag_dist_rot_pin = 14
+   # gpio_config.mag_dist_rot_pin = 14
     if gpio_config.debug:
         gpio = MockGPIO()
     else:

playgrounds/mag_to_feeder_moving_sequence.py

@@ -0,0 +1,154 @@
+import time
+from robot_control.src.robot.mag_distributor import MagDistributor
+from robot_control.src.api.gpio import PiGPIO
+from robot_control.src.utils.config import ConfigParser
+import asyncio
+import time
+import sys
+
+# Assign two servo pins (update as needed)
+SERVO_PINS = {1: 6, 2: 13}  # Example: GPIO6 and GPIO13
+
+gpio = PiGPIO([], list(SERVO_PINS.values()))
+
+# Load config and create mag distributor for stepper control
+try:
+    config_parser = ConfigParser()
+    mag_dist = MagDistributor(config_parser.config, gpio)
+except Exception as e:
+    print(f"Error: Could not initialize mag distributor: {e}")
+    sys.exit(1)
+
+def move_servo(servo_number, angle, speed):
+    """Move servo using PiGPIO's smooth function"""
+    pin = SERVO_PINS.get(servo_number)
+    if pin is None:
+        print(f"Servo {servo_number} not configured.")
+        return False
+    
+    print(f"S {servo_number} {angle} {speed} - Moving servo {servo_number} (GPIO {pin}) to {angle}° at speed {speed}")
+    try:
+        gpio.set_servo_angle_smooth(pin, angle, speed)
+        print("Servo move complete.")
+        return True
+    except Exception as e:
+        print(f"Error moving servo: {e}")
+        return False
+
+def move_stepper(pos_mm, rot_deg, speed=7):  # Increased default from 100 to 300 mm/min
+    """Move stepper motor using mag distributor"""
+    print(f"M {pos_mm} {rot_deg} {speed if speed else ''} - Moving stepper to position {pos_mm}mm, rotation {rot_deg}° at {speed} mm/min")
+    try:
+        if speed is not None:
+            # Convert speed to step delay if needed
+            step_delay = mag_dist._speed_to_step_delay(speed)
+            mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg, step_delay)
+        else:
+            mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg)
+        print("Stepper move complete.")
+        return True
+    except Exception as e:
+        print(f"Error moving stepper: {e}")
+        return False
+
+async def home_system():
+    """Home the mag distributor system"""
+    print("H - Starting homing sequence...")
+    try:
+        await mag_dist.home()
+        print("Homing completed successfully.")
+        return True
+    except Exception as e:
+        print(f"Homing failed: {e}")
+        return False
+
+def execute_sequence():
+    """Execute the predefined sequence of movements"""
+    print("Starting Mag_to_Dist_to_Rob sequence...")
+    print("=" * 50)
+    
+    # Define the sequence of commands (including homing at the start)
+    commands = [
+        ("S", 1, 60, 1000),     # S 1 40 1000
+        ("H",),                 # Homing sequence
+        ("M", -222, 0, 10),    # M -223 0 200 (increased speed from 100 to 200)
+        ("S", 1, 5, 250),       # S 1 0 250
+        ("S", 1, 60, 250),      # S 1 40 250
+        ("M", -46, 0, 10),     # M -46 0 150 (increased speed from 100 to 150)
+        ("S", 2, 15, 500),      # S 2 35 250
+        ("S", 1, 153, 200),     # S 1 153 200
+        ("S", 2, 88, 500),     # S 2 90 1000
+        ("S", 2, 130, 150),     # S 2 120 150
+        ("S", 1, 60, 1000),     # S 1 40 1000
+        # Defeeder pos -6 angle 120
+    ]
+    
+    success_count = 0
+    total_commands = len(commands)
+    
+    for i, command in enumerate(commands, 1):
+        print(f"\nStep {i}/{total_commands}:")
+        
+        if command[0] == "H":  # Homing command
+            success = asyncio.run(home_system())
+        elif command[0] == "S":  # Servo command
+            _, servo_num, angle, speed = command
+            success = move_servo(servo_num, angle, speed)
+        elif command[0] == "M":  # Motor/stepper command
+            _, pos_mm, rot_deg, speed = command
+            success = move_stepper(pos_mm, rot_deg, speed)
+        else:
+            print(f"Unknown command type: {command[0]}")
+            success = False
+        
+        if success:
+            success_count += 1
+        else:
+            print(f"Command {i} failed!")
+            response = input("Continue with next command? (y/n): ").strip().lower()
+            if response != 'y':
+                print("Sequence aborted by user.")
+                break
+        
+        # Wait 1 second between commands (except for the last one)
+        if i < total_commands:
+            print("Waiting 1 second...")
+            time.sleep(2)
+    
+    print("\n" + "=" * 50)
+    print(f"Sequence completed: {success_count}/{total_commands} commands successful")
+    
+    return success_count == total_commands
+
+def main():
+    """Main function"""
+    print("Mag_to_Dist_to_Rob - Automated Movement Sequence with Homing")
+    print("This script will execute a predefined sequence starting with homing.")
+    print("\nSequence includes:")
+    print("1. Homing sequence (H)")
+    print("2. Servo and stepper movements as specified")
+    print("\nPress Enter to start the sequence, or Ctrl+C to exit...")
+    
+    try:
+        input()
+        success = execute_sequence()
+        
+        if success:
+            print("\n✓ All movements completed successfully!")
+        else:
+            print("\n⚠ Some movements failed. Check the output above.")
+            
+    except KeyboardInterrupt:
+        print("\n\nSequence interrupted by user.")
+    except Exception as e:
+        print(f"\nUnexpected error: {e}")
+    finally:
+        # Cleanup
+        try:
+            gpio.cleanup()
+            print("GPIO cleanup completed.")
+        except:
+            pass
+
+if __name__ == "__main__":
+    main()

robot_control/config/config.yaml

@@ -78,7 +78,7 @@ defeeder_magazines:
     name: error
 
 mag_distributor:
-    debug: True
+    debug: False
     max_speed_mmmin: 10000
     home_speed_mmmin: 2000
     length_mm: 500
@@ -120,15 +120,17 @@ gpio:
   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_pos_dir_pin: 11
+  mag_dist_pos_step_pin: 9
+  mag_dist_pos_en_pin: 10
+  mag_dist_pos_limit_pin: 16
 
   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_servo_pin: 6
+ 
 
   mag_dist_sensor_pin: -1
 

robot_control/src/api/gpio.py

@@ -43,11 +43,19 @@ class GPIOInterface(ABC):
         pass
 
 class PiGPIO(GPIOInterface):
+    """
+    Enhanced PiGPIO implementation with chunked stepper motor control.
+    This prevents "Connection reset by peer" errors when doing large moves.
+    """
     def __init__(self, in_pins: list=[], out_pins: list=[]):
         super().__init__(in_pins, out_pins)
         self.pi = pigpio.pi()
         if not self.pi.connected:
             raise RuntimeError("Could not connect to pigpiod. Is it running? Try: 'sudo systemctl start pigpiod'")
+        
+        # Maximum steps per chunk to prevent connection reset
+        self.MAX_STEPS_PER_CHUNK = 2500 # Increased from 1000 to 2500
+
         for pin in out_pins:
             self.pi.set_mode(pin, pigpio.OUTPUT)
             self.pi.write(pin, 0)  # Ensure output pins are off initially
@@ -113,34 +121,171 @@ class PiGPIO(GPIOInterface):
 
     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
+        Perform a step operation on a stepper motor with chunking to prevent connection reset.
+        
+        CHUNKING STRATEGY:
+        - Large movements (>MAX_STEPS_PER_CHUNK) are divided into smaller chunks
+        - Each chunk is sent as a separate waveform to prevent overwhelming pigpio
+        - Small delay between chunks allows pigpio to process commands
+        
+        Args:
+            dir_pin: Direction pin
+            step_pin: Step pin to trigger steps
+            steps: Total number of steps to perform
+            step_delay_us: Delay between steps in microseconds
+            direction: True for forward, False for reverse
         """
-        # Set direction
+        #print(f"[CHUNKED] Moving {steps} steps, direction: {direction}, delay: {step_delay_us}us")
+        
+        # Set direction once at the beginning
         self.set_pin(dir_pin, 1 if direction else 0)
+        
+        remaining_steps = steps
+        chunk_count = 0
+        
+        while remaining_steps > 0:
+            # Determine chunk size
+            chunk_size = min(remaining_steps, self.MAX_STEPS_PER_CHUNK)
+            chunk_count += 1
+            
+            print(f"  Chunk {chunk_count}: {chunk_size} steps (remaining: {remaining_steps})")
+            
+            # Create pulse waveform for this chunk
+            pulses = []
+            for _ in range(chunk_size):
+                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
 
-        # 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
+            # Send chunk waveform
+            self.pi.wave_clear()
+            self.pi.wave_add_generic(pulses)
+            wave_id = self.pi.wave_create()
 
-        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)
+                
+                # Wait for waveform to complete
+                while self.pi.wave_tx_busy():
+                    time.sleep(0.01)  # Small sleep while waiting
+
+                self.pi.wave_delete(wave_id)
+                
+                # Small delay between chunks to prevent overwhelming pigpio
+                if remaining_steps > chunk_size:  # Not the last chunk
+                    time.sleep(0.001)  # 50ms delay between chunks
+                    
+            remaining_steps -= chunk_size
+
+        # Clear direction pin
+        self.set_pin(dir_pin, 0)
+        print(f"[CHUNKED] Movement complete: {steps} steps in {chunk_count} chunks")
 
-        if wave_id >= 0:
-            self.pi.wave_send_once(wave_id)
-            while self.pi.wave_tx_busy():
-                time.sleep(0.1)
+    def move_to_position_mm(self, dir_pin: int, step_pin: int, target_position_mm: float, 
+                           current_position_mm: float, steps_per_mm: float, 
+                           speed_mmmin: float = 1000) -> float:
+        """
+        Move motor to target position in mm using the chunked stepper control.
+        
+        Args:
+            dir_pin: Direction pin
+            step_pin: Step pin
+            target_position_mm: Target position in millimeters
+            current_position_mm: Current position in millimeters
+            steps_per_mm: Steps per millimeter conversion factor
+            speed_mmmin: Speed in mm/min
+            
+        Returns:
+            float: New current position in mm
+        """
+        delta_mm = target_position_mm - current_position_mm
+        if abs(delta_mm) < 0.001:  # Less than 1 micrometer
+            print(f"Already at target position {target_position_mm} mm")
+            return current_position_mm
+
+        steps_total = int(abs(delta_mm) * steps_per_mm)
+        direction = delta_mm > 0
+        
+        # Convert speed from mm/min to step delay in microseconds
+        steps_per_sec = speed_mmmin * steps_per_mm / 60.0
+        step_delay_us = max(10, int(1_000_000 / (2 * steps_per_sec)))  # *2 for high+low pulse
+        
+        print(f"Moving from {current_position_mm:.2f} mm to {target_position_mm:.2f} mm")
+        print(f"  Distance: {delta_mm:.2f} mm = {steps_total} steps")
+        print(f"  Speed: {speed_mmmin} mm/min = {step_delay_us} µs delay")
+        
+        # Use existing chunked do_step method
+        self.do_step(dir_pin, step_pin, steps_total, step_delay_us, direction)
+        
+        return target_position_mm
+
+    def home_axis(self, dir_pin: int, step_pin: int, limit_pin: int, 
+                  speed_mmmin: float = 200, max_travel_mm: float = 200, 
+                  steps_per_mm: float = 80) -> bool:
+        """
+        Home an axis using limit switch with chunked movement.
+        
+        Args:
+            dir_pin: Direction pin
+            step_pin: Step pin  
+            limit_pin: Limit switch pin (active LOW)
+            speed_mmmin: Homing speed in mm/min
+            max_travel_mm: Maximum travel distance before giving up
+            steps_per_mm: Steps per millimeter conversion
+            
+        Returns:
+            bool: True if homing successful, False if failed
+        """
+        print(f"Homing axis - limit pin {limit_pin}")
+        
+        # Convert speed to step delay
+        steps_per_sec = speed_mmmin * steps_per_mm / 60.0
+        step_delay_us = max(10, int(1_000_000 / (2 * steps_per_sec)))
+        
+        max_steps = int(max_travel_mm * steps_per_mm)
+        
+        # Set direction (adjust based on your setup)
+        self.set_pin(dir_pin, 1)  # Move toward limit switch
+        
+        steps_moved = 0
+        chunk_size = 100  # Small chunks for homing
+        
+        print(f"Moving toward limit switch (max {max_steps} steps)...")
+        
+        while steps_moved < max_steps:
+            # Check limit switch before each chunk
+            if self.get_pin(limit_pin) == 0:  # Active LOW limit switch
+                print(f"Limit switch triggered after {steps_moved} steps")
+                self.set_pin(dir_pin, 0)  # Clear direction
+                return True
+            
+            # Move a small chunk
+            remaining = min(chunk_size, max_steps - steps_moved)
+            
+            # Create small waveform chunk for homing
+            pulses = []
+            for _ in range(remaining):
+                pulses.append(pigpio.pulse(1 << step_pin, 0, step_delay_us))
+                pulses.append(pigpio.pulse(0, 1 << step_pin, step_delay_us))
 
-            self.pi.wave_delete(wave_id)
+            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.001)
+                self.pi.wave_delete(wave_id)
+            else:
+                print("Failed to create homing waveform")
+                self.set_pin(dir_pin, 0)
+                return False
+                
+            steps_moved += remaining
+            
+        print(f"Homing failed - no limit switch found after {max_steps} steps")
         self.set_pin(dir_pin, 0)
+        return False
 
     def cleanup(self):
         if self.pi.connected:

robot_control/src/robot/mag_distributor.py

@@ -15,10 +15,12 @@ class MagDistributor:
         gpio_conf = config.gpio
         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.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_servo_pin = gpio_conf.mag_dist_rot_servo_pin
+        #print(self.rot_servo_pin)
 
         # Endstop pins
         self.linear_limit_pin = gpio_conf.mag_dist_pos_limit_pin
@@ -58,6 +60,21 @@ class MagDistributor:
         step_delay_s = 1.0 / steps_per_s
         return int(step_delay_s * 1000)  # ms
 
+    def move_servo(self, rot_servo_pin, angle):
+        """
+        Move servo using PiGPIO's smooth function
+        """
+        if rot_servo_pin is None:
+            print(f"Servo {rot_servo_pin} not configured.")
+            return False        
+        try:
+            gpio.set_servo_angle_smooth(pot_servo_pin, angle)
+            print("Servo move complete.")
+            return True
+        except Exception as e:
+            print(f"Error moving servo: {e}")
+            return False
+
     async def home(self):
         """
         Home both axes using endstops.
@@ -66,12 +83,12 @@ 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)
+        #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):
+    async def _home_axis(self, dir_pin, step_pin, endstop_pin, axis='linear', max_steps = 10000, step_delay=100):
         """
         Home a single axis by moving until the endstop is triggered.
         """
@@ -79,7 +96,7 @@ class MagDistributor:
             return
         # Move in negative direction until endstop is triggered
         for _ in range(max_steps):
-            if self.gpio.get_pin(endstop_pin):
+            if not self.gpio.get_pin(endstop_pin):
                 break
             self.gpio.do_step(dir_pin, step_pin, 1, step_delay, direction=True)
             await asyncio.sleep(0.001)
@@ -87,7 +104,7 @@ class MagDistributor:
             raise RuntimeError(f"{axis} axis homing failed")
         self.logger.info(f"{axis} axis homed.")
 
-    def move_mag_distributor_at_pos(self, pos_target, rot_target, step_delay=None):
+    def move_mag_distributor_at_pos(self, pos_target, rot_target, step_delay=200):
         """
         Move the magazine distributor to the specified linear and rotational position.
         """
@@ -105,11 +122,11 @@ class MagDistributor:
             self.gpio.do_step(self.linear_dir_pin, self.linear_step_pin, linear_steps, step_delay, linear_dir)
             self.curr_pos_mm = pos_target
 
-        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
+        # 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

@@ -132,6 +132,7 @@ class GPIOConfig(BaseModel):
     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_servo_pin: int = Field(ge=-1, le=27)
 
     mag_dist_sensor_pin: int = Field(ge=-1, le=27)